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ABSTRACT 


The major problem addressed by this research is how to improve an existing real-time 
software system’s readability, maintainability, stability and portability using reengineering 
techniques. A fundamental portion of the Model-based Mobile robot Language (MML) 
was the real-time system chosen as the basis for this study. 

The approach taken was to create a new system design. The new design was based on 
system specifications obtained by conducting static and dynamic analysis on the existing 
system. 

The results are that a new core system was implemented using a design that focused on 
creating independent software sub-systems while encapsulating data. Hardware 
dependencies were localized and assembly code minimized. The new system 1s easier to 


understand and modify and is portable to other hardware platforms. 
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I. INTRODUCTION 


A. PROBLEM STATEMENT 
The problem this thesis solves is how to reengineer MML, a real-time control system 
for the Yamabico-11 autonomous robot, while improving system stability, maintainability 


and portability. 


B. BACKGROUND 

MML is a real-time system under development for the Yamabico-11 robot. The goal 
of the project is to create a robot-independent, high-level language for mobile robot control. 
The language contains sets of library functions to handle geometry, motion, sonar, and I/O. 
These routines could then be used by developers to program a robot’s movement without 
the requirements of low-level motion control understanding. 

In the class of motion control functionality, MML originally used a sequence of 
configurations to describe a vehicles desired path. A configuration represents the robot’s 
current position and orientation in a 2D world. Current research involves expanding the 
language to describe motion control using directed path segments as well. Each directed 
path is defined by a point that lies on the segment, the orientation of the segment in the 2D 
world and the segment’s curvature. 

The status of the MML system typifies that of many software systems in existence 
today. Roughly 50 to 80 percent of a software organization’s resources are spent 
maintaining existing systems. Many of these systems were developed without complete 
specifications or documentation by analysts and programmers who are no longer with the 
organization. Analysts and programmers with incomplete system knowledge are then 
required to perform the necessary maintenance [Yourdon 93]. 

As a research system, MML evolved through modifications made from several 


graduate students. Many of these changes were unstructured and made use of global 


variables. At times, particular functions or sections of code were altered simultaneously by 
different developers. The result is an unstable system that is extremely difficult to maintain. 

Some organizations are investing resources into software reengineer techniques, 
hoping to “eam maintenance efforts and costs dedicated to these existing systems. There 
are three fundamental approaches to reengineering a system. The first technique, called 
restructuring, reorganizes unstructured source code into a modular form. The new form 
remains functionally equivalent to the older version. The second method is to reengineer 
the system. The goal of this plan is to replace existing code with newer versions, possibly 
written in a higher level language. This is done gradually by the maintenance programmers 
each time maintenance is required. Reverse engineering is the third technique. The 
objective of this procedure is to reconstruct the design, and/or the specifications from the 


existing source code [Yourdon 93]. 


C. OVERVIEW 


This thesis shows a procedure for reengineering an existing real-time system, MML- 
10. The first step in this process is to determine the functionality of the existing system. 
Chapter two analyzes MML-10’s software system, while chapter three covers the hardware 
components of the Yamabico-11 robot. Generating a new design is the second step and is 
covered in chapter four. The third step is to implement and test the new design. Chapter five 
details Yamabico-11’s software development environment. The design implementation 
and results are discussed in chapter six. Chapter seven presents the conclusions and future 


recommendations. 


Il. YAMABICO SOFTWARE SYSTEM ARCHITECTURE 


Prior to reengineering an existing software system, the maintenance programmer must 
have a thorough understanding of that system’s logical design. Unless the programmer was 
involved with the system’s original implementation, this is only achieved by reviewing the 
software’s specifications and/or documentation. For systems lacking complete 
specifications and /or documentation, such as MML, it may be necessary to perform reverse 
engineering techniques to recover this information. Since current CASE technology 
focuses on developing new systems, these tools can provide limited support, but not at a 
high level [Yourdon 93]. 

Two of the most effective methods for reconstructing system specifications and/or 
documentation are static and dynamic analysis. The results of these two techniques form a 
logical description of the system. This description can then be translated into physical 
depictions, such as data flow diagrams or state transition diagrams [Yourdon 89]. The 


context diagram of the current MML system is shown in Figure 1. 
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Figure 1: MML System Overview 


A. STATIC ANALYSIS OF MML-10 

Static analysis is the process of gaining an understanding of a software system through 
source code examination. The goal of this phase is to recover enough information to create 
a logical picture of the existing system. One important technique involves the tracing of 
function and procedure calls. When performing this procedure, it 1s not desirable to produce 
a complete or detailed trace as this only wastes time and resources. 

Another important technique involves variable tracing. The emphasis during this 
operation should be on tracing parameters passed to functions and procedures, any values 


they return and references to global variables. 


1. User vs. Kernel 


As shown in Figure 2, the MML system 1s composed of two distinct parts: the 


kernel module and a user program. The kernel contains the low level routines required to 
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Figure 2: MML System 


control movement and to perform input/output. The low level details are hidden from 
developers through the MML command set. A user program consists of MML command 
calls and C constructs to describe one or more motion behaviors. Each command either 


requests status information from the kernel or commands a desired behavior [Scott 93]. 


Each module is downloaded to the robot separately. Robot operation is only 
possible after both modules are loaded. Program execution starts with the kernel. The 
kernel initializes the sub-systems and then passes control to the user program. The user 
program maintains primary control until termination. However, the kemel’s control 
processes will interrupt execution for short durations. This design allows a user to quickly 


alter the behavior of the robot by simply changing the user program used by the kernel. 


2. Kernel System 

Figure 3 is a modified data flow diagram showing the kernel as a collection of 
control type processes. Each control process, indicated by a dashed circle, is associated 
with a distinct hardware generated interrupt. The interrupts are prioritized through the 
hardware’s configuration. The current CPU provides eight priority levels. When an 
interrupt is generated, the associated control process assumes control of the CPU, provided 
it is not interrupting a higher level process. Otherwise, it will wait until all higher priority 
interrupts are serviced before taking control. The interrupt level of each process is indicated 
by the value within parentheses. 

Since motion control is responsible for maneuvering the robot, receives the 
highest priority given to the control processes, interrupt level four. It controls movement 
by first estimating the robot’s current odometry configuration through dead reckoning. This 
estimate is then used to calculate the necessary linear and rotational velocities. These 
calculations are based on the motion required to follow the current path segment. Finally, 
these velocities are translated into pulse width modulation commands and sent to the 
motors controlling the robot’s wheels. This process, called the motion control cycle, repeats 
every 10 milliseconds and requires approximately 2.5 milliseconds to execute 
[MacPherson 93]. 

The input/output process receives the next highest priority at interrupt level 


three. It uses interrupt driven input and output to effect information transfers between the 
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Figure 3: Kernel System 


robot and the onboard console device. It also controls the transfer of data between the robot 
and a Unix workstation [MacPherson 93]. 

Interrupt priority level two signals the robot’s sonar process. This process 
collects sonar range information used for obstacle avoidance. The process requires 240 


microseconds to complete and is repeated every 24 milliseconds [DeClue 93]. 


3. Motion Control Process 
As depicted in Figure 4, the major responsibilities of motion control are 


processing MML commands and pathtracking. Each MML command 1s characterized as 
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Figure 4: Motion Control Process 


either an immediate or sequential command. Both types induce changes to one or more 


control parameters. Immediate commends induce the changes at the instant they are called 


from within the user program. However, when a sequential command is called from the 
user program, it is added to an instruction queue. Each sequential command is then 
executed only after the previous sequential command is finished. Immediate command 
execution and storage of sequential commands into the instruction queue is independent 
from the motion control cycle. 

For path tracking, a three step process is executed every motion control cycle. 
First, Yamabico updates its current odometry configuration, position and orientation, by 
reading hardware registers associated with wheel movement. Next, this information is used 
to calculate the robot’s next intended movement using control rules. Early motion 
description methods in MML compared the current configuration against a reference 
configuration [Kanayama 91]. However, recent research has proven that smoother motion 
control is achieved by comparing the current configuration against a reference path 
segment [MacPherson 93]. Finally, the intended movement is translated into pulse width 
modulation values and sent to the motor control board. 

A path segment is either a straight line, circular arc, parabola or cubic spiral. 
Yamabico will track a sequence of these path segments added to the instruction queue by 
calculating a transition point from one segment to the next. Once the transition point is 
reached, the robot will begin following the next path segment. 

A secondary task of motion control is data logging. This purpose of this function 
is to record control data every motion control cycle. The collected data is used by the 


system programmers for debugging. 


4. VO Process 


As previously mentioned, the input/output process is responsible for transferring 
information between the robot and either the Unix workstation or the console. 
Communications between the robot and the Unix workstation are accomplished by polling 


a serial port mapped to a specific memory location. 


Transferring information with the console is also memory mapped, however this 
process is interrupt driven. During ou put, data is stored in a 1024 character circular buffer. 
The interrupt sequence is then initiated by sending a null character directly to the console. 
After the character is received by the console, an interrupt causes the next character in the 
buffer to be sent. After this character is written to the console, another interrupt 1s 
generated. This process continues until the buffer is empty and a command to terminate the 


interrupt cycle is sent to the port. Input is handled in a similar fashion. 


5. Sonar Process 
As mentioned earlier, the sonar process is responsible for recording sonar range 
returns. This information is made available to user programs for obstacle avoidance. The 
returns can also be used for automated cartography by building line segments using a least 


squares linear fitting algorithm [DeClue 93][MacPherson 93]. 


B. DYNAMIC ANALYSIS OF MML-10 


When static analysis provides insufficient insight to a system’s behavior, dynamic 
analysis should be used. Dynamic analysis is the process of tracing a system during 
execution. Using dynamic analysis, a maintenance programmer can follow the path of 
execuwion, monitor access to a particular memory location or alter the storage values. This 
is particularly useful when trying to locate the cause of a system crash [Yourdon 93]. 

Tracing MML-10 has proven to be very difficult. Two reasons are the system’s low 
readability and high degree of coupling. This is the result of using inappropriate modularity 
techniques. These and other system characteristics are described below. Another reason is 
the difficulty associated with using a debugger due to the timing constraints of a real time 


system. 


C. SYSTEM CHARACTERISTICS 


A by-product of conducting static and dynamic analysis is knowledge of a system’s 


software characteristics. Software characteristics are those attributes used to describe the 


quality of a system’s design and implementation. The following list of attributes is used to 
describe the internal makeup of a system and is directly related to the effort required to 


perform system maintenance. 


1. Coupling 

When one module references a symbolic address defined outside of that module, 
a connection (or interdependency) is created between the module with the reference and the 
module with the definition. Coupling describes the types and strength of these connections 
between modules. References to internal data elements or data structures is known as 
common coupling since code segments are referencing a common data area. Another form 
of coupling is control coupling. This exists when control switches such as flags are used 
between modules. The purpose of these flags is to change or modify the behavior or actions 
of a routine. Low coupling exists when references between modules are limited to 
procedure and/or function names. 

As coupling increases, a system is more difficult to understand and maintain. 
Therefore, it is desirable to reduce coupling by reducing the references to another module’s 
internal elements. One method for reducing common coupling is to bring the externally 
referenced elements inside the module. However, this only works if the elements are not 
referenced by other sections of code. Control coupling can be reduced by splitting the 
routines effected by the flags into seperate procedures or functions. Then the calling routine 


would make seperate calls to the new routines [Stevens 74]. 


2. Cohesion 
When more than one code segment references the same element, these segments 
are related. Cohesion measures the strength of relationships between code segments within 
the same module. It is desirable that modules exhibit strong cohesion. For that reason, 
related segments should be collected in the same module that contains the referenced 


element [Stevens 74]. 
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3. Modifiability 
A system is modifiable if changes can be made to one segment of code without 
generating adverse side effects in another segment. Another name for this attribute is 
stability. The degree that a system is modifiable is directly related to the system’s measure 
of coupling and cohesiveness. A modifiable system is produced through the application of 
sound implementation techniques to a solid design [Yourdon 80]. Once produced, a 


modifiable system is easily changed and maintained. 


4. Modularity 
Modularity is defined as the partitioning of the system into small segments. 
Creating a modular system also begins during the design phase. A major goal of this 
process is to design each segment around a particular logical function performed by the 
system [Parnas 79]. This produces a system exhibiting strong cohesion. Another goal is to 
minimize the amount of coupling. This is done by using a clean and concise interface with 
data encapsulation; the hiding of data elements. The success of this process is measured 


through the ease of implementation and maintenance. 


5. Readability 
Occasionally, original developers are no longer available after a system’s 
completion [Yourdon 80]. For this reason, systems need to exhibit the same behavior 
during operation as expressed in the source code. This characteristic is termed readability. 
Small modules with independent, well defined and clearly documented behavior are the 


most readable. Therefore it is important for modules to exhibit simplicity and consistency 


[Scott 93]. 


6. Robustness 


Systems that can detect errors (or exceptions) and recover are considered robust 


or fault tolerant. This requires the addition of exception handling functions and procedures 


to the system. These code segments allow the system to process exceptional conditions 


such as division by zero or value out of limits [Scott 93]. 


D. CHARACTERISTICS OF MML-10 


MML-10 was an attempt to restructure the system by collecting related functions into 
seperate modules. Although this strengthened cohesion, improvements were limited due to 
the usage of global variables [Scott 93]. Almost all of the global variables are declared and 
initialized in the module main.c. However, most global variables are actually used or 
referenced in one or more different modules. There are also several instances in MML-10 
where global flags set in one module effect the behavior of a function in another. Collecting 
the data elements into one common area also eliminated the requirements for clean 
interfaces between modules. This has resulted in a tightly coupled system. 

To reduce system coupling, the required global variables should be re-located to the 
modules that reference them. These variables should then be encapsulated with the 
development of module interfaces. 

The MML-10 source code is difficult to read. One reason is poor documentation. 
MML-10’s comments are minimal and often give incomplete descriptions of the code. 
Also, the short motion control theory nomenclature is used for the system’s global variable 
names. Source code documentation can be improved using a combination of well placed, 
informative comments and descriptive symbolic names. 

Improper use of pointers is another reason MML-10 tends to be confusing and difficult 
to understand. In many places, global pointers reference global variables while some 
assignments are improperly type casted. In other places, pointers are used in a cryptic 
manner. For example, instead of using standard indexing to sequence through an array, 
pointers are used. These practices should be avoided. 

One of the goals of the MML project was to modify the system, making it easily ported 


to other hardware platforms. This involved converting the current system to a portable 
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language, such as ANSI C or C++, while minimizing the amount of required assembly 
code. It also implies that hardware dependencies should be local to a few modules. 
However, attempts to reengineer MML-10 by replacing existing non-ANSI C or 


assembly code with ANSI C failed. For example, Figure 5 shows the addition of two 


Module main.asm.s 


long SEAN 


movl #0 SEAN 





Figure 5: Introduction of a Unique Variable 


Statements to an assembly language module belonging to the system. The first statement, 
.long SEAN, simply introduces a unique variable, while the second statement, movl 
#0,SEAN, initializes it. Prior to the change, the system executed properly. After adding the 
two program statements, the resulting system would no longer function. No other changes 
were made. Similar results occurred when adding or deleting output statements. To create 


a stable system, MML should be re-implemented using solid engineering techniques. 
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Il. YAMABICO HARDWARE ARCHITECTURE 


The Yamabico-11 is a collection of hardware sub-systems assembled on an aluminum 


frame. As shown in Figure 6, these hardware sub-systems include: a CPU system, a wheels 
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Figure 6: Yamabico-11 Computer Architecture 


system, a sonar system, a serial system, a Unix host system and a console system used as 
the communications interface. 

The chassis houses two 12-volt rechargeable batteries. These batteries power all sub- 
systems except for the console. The chassis rests on four spring-loaded castering wheels for 
balance while two wheels attached to the chassis in a differential arrangement control robot 


movement. The actual robot 1s shown in Figure 7. 
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Figure 7: Yamabico-11 Mobile Robot 
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A. THE CPU SYSTEM 

The current CPU sub-system uses a VME7120 32-bit motherboard. The board contains 
a Motorola based MC68020 CPU operating at 16MHz and a MC68881 co-processor for 
floating point arithmetic [VCM 86]. The system also includes one megabyte of dynamic 
memory and a ROM based VME7920 Debugging Package. The debugger is used as the 
monitor program when the system is powered on [VDP 86]. An upgrade of the sub-system 
to a SPARC-4 processor board containing 16 megabytes of dynamic memory is planned. 

One of the main functions of the CPU is to manage the interrupt driven requests from 
the other subsystems. As mentioned earlier, the MC68020 CPU provides eight levels of 
interrupts. The assignment of each interrupt level is summarized in Table 1 [MacPherson 
93]. To handle the interrupts, the MC68020 uses a special purpose register to hold the 

TABLE 1: MML SYSTEM TASK PRIORITY 


Interrupt Interrupt Duration 
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address of a vector table. This table is essentially an array of 256 elements, where each 












element can store the address of an exception/interrupt handler. When an exception/ 
interrupt is signaled, the CPU saves the current status word and the status word is modified 
for interrupt processing. Next, an index value into the vector table is obtained from the 


interrupting device. Then the current context is saved on a supervisor stack. Finally, 
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execution resumes at the address located in the vector table cell specified by the index 
value. When the interrupt handler is finished, the rte assembly instruction restores the 


processor to the state prior to the interrupt [MC68020 85]. 


B. THE SERIAL SYSTEM 

The serial sub-system is composed of two VME8300 Quad Serial Port Boards. Each 
board contains a VME bus interface and two serial communications controllers. Each 
controller manages two serial ports. Associated with each port is a timer/counter that may 
be used for baud rate generation or asynchronous/synchronous interrupt control. There also 
exists a fifth timer/counter that is primanly used to generate synchronous interrupts. 


However, each board contains only two latches for a maximum configuration of two levels 


of interrupts per board [VQS 86]. Figure 8 shows the conceptual layout of a serial board. 


Controller 1 Controller 2 


Figure 8: Serial Board Conceptual Diagram 





The ports and timers are configured by reading from and writing to registers located 
on the serial boards. These registers are memory mapped and are accessed through absolute 


addressing. 


C. THE WHEELS SYSTEM 


The wheel sub-system consists of two independent DC motors that drive the two 


wheels in either the forward or reverse direction, controlling the robot’s movement. 


Braking can also be applied to the motors. Each motor has an associated shaft encoder that 
is used to determine distance traveled, speed, and to make odometry correction. 

A dual axis controller board serves as the interface between the wheel motors, the shaft 
encoders and the CPU. It contains memory mapped registers that are accessed through 
absolute addressing. These registers are used to enable/disable the motors, read the shaft 
encoders and to send pulse width modulation values. A pulse width modulation value is an 
eight-bit integer that determines the strength of a short electrical pulse sent to a motor to 
create movement. Since each pulse is short, constant pulse generations are required to 


produce smooth, continuous motion. 


D. THE SONAR SYSTEM 

The sonar sub-system contains twelve 40kHz ultrasonic sensors. These sensors are 
used to gather sonar return information from the forward/rear, lateral or diagonal 
directions. Three control boards are used to control the sensors and collect return 


information. A VME bus card is used as the interface between the control cards and the 


CPU [DeClue 93]. 


E. THE UNIX HOST SYSTEM 

The Unix host system is a Sun-3 workstation using SunOS 4.1.1. It is connected to the 
robot through a serial port on the first serial board. The host is used to develop software that 
will be transferred to the robot. It also accepts collected data from the robot. The interface 


is easily disconnected during robot operation, allowing full motion freedom. 


F. THE CONSOLE SYSTEM 


A Macintosh Power Book is the main component of the console sub-system and is the 
only input/output device when Yamabico operates as a self-contained robot. The Power 
Book is connected to the robot through a serial port on the second serial board and provides 
the interface between the user and the robot’s debug monitor through a software 


communications package. It also has its own rechargeable power supply. 


IV. SYSTEM DESIGN 


A. DESIGN GOALS 

Initially, the goal of the reengineering process was to add structure to MML-10, 
replacing non-ANSI C code with ANSI C. However, it was soon realized that a 
monumental effort would be required. Therefore, a plan to re-design the core system was 
initiated. 

Since MML is used in conducting research by many people, the new design was based 
on three requirements. First, the system must be easy to read and easy to maintain. 
Therefore, modularity is a primary goal of the new design. As discussed earlier, a modular 
system exhibits strong cohesion and loose coupling. 

MML will see many modifications and changes as the system continues to evolve and 
expand. Therefore, the second requirement is system stability. The creation of a stable 
system begins by applying solid implementation techniques to a modular design. Some 
important techniques that promote stability are discussed in Chapter VI. 

The third requirement 1s portability. When the Yamabico’s processor is upgraded, the 
new software should only require minor changes. This implies that the assembly code 
should be minimized and the hardware dependencies localized. A portable system also 
promotes software re-use for future platforms. 

A secondary objective is to create an object oriented type design. This will encourage 


the smooth transition to an object oriented language such as C++. 


B. MODELING NOTATION 


When modeling software, the goal of the notation is to produce a picture of the 
software system that is clear and easy to interpret. It must simplify the system by 
highlighting the important features, while hiding the details [Constantine 94]. 

The notation chosen to model the new design is shown in Figure 9. A software system 


or hardware component is represented by a solid box. Interrupt handlers are shown as 


dashed circles while solid circles indicate system processes. In some systems, the interface 
is not yet finalized. For this reason a single solid circle may represent more than one 
process. Solid arrows depict messages between systems and/or processes, while dashed 
arrows display the signals used to activate interrupt handlers and system control processes. 


Finally, data stores are indicated by two solid parallel lines. 
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Figure 9: Design Notation 


C. SYSTEM DESIGN 


1. System Overview 
A model of MML is illustrated in Figure 10. This diagram depicts MML as a 
network of five sub-systems: a CPU system, a motion control system, a terminal system, a 
sonar system and a user program. This overview is slightly more complex than the views 
depicted in Figure 1 and Figure 2. However, this approach helps produce the desired object 
oriented design where each sub-system is treated as an object. This approach results in a 


simpler system design overall. 


2. CPU System 


The CPU system, shown in Figure 11, is the primary system. One objective of 


this system is to initialize the other sub-systems. If the system is implemented with an 
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Figure 10: System Network 


object oriented language, C++ for example, this can be accomplished when the object is 
instantiated. However, if a non-object oriented language is used, such as ANSI C, then each 
sub-system 1s required to have an initialization process. In the later case, the CPU system 
must specifically call these initialization routines for each sub-system. 

Since the interrupt control mechanism is CPU dependent, the interrupt handling 


routines and the mechanism setup is part of the CPU system. The objective of this operation 
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Figure 11: CPU System 


is to match the interrupt handling functions to the appropriate interrupt signals. However, 


the interrupt priority levels are still based on the hardware configuration. 


The third function of the CPU system 1s to pass control to the user program. This 


is done after the sub-systems have been initialized and the interrupt control process has 


been properly established. 


Serial System 


Illustrated in Figure 12, the serial system shields the hardware details of the 


VME8300 serial boards from the other sub-systems in MML. Each serial port or timer must 


be properly initialized prior to its first use. This is accomplished through a pair of 
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initialization processes. A set of interface routines are required by each port to read and 
write character data. These interface routines are modeled after examples found in the 
user’s manual [VQS 86]. Since this system directly accesses the hardware, it must NOT be 
optimized. If optimization techniques are used, the initialization routines will improperly 


setup the ports. 
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Figure 12: Serial System 


The VME8300 boards ability to generate synchronous interrupts is a major 
benefit. They are required to generate control signals to activate other sub-systems. For this 
reason the serial system is considered to be a component of the CPU system rather than an 


MML sub-system (see Figure 11). 


Zs 


4. Motion Control System 


The motion control system depicted in Figure 13 serves two primary purposes. 
First, it provides the interface for MML’s immediate and sequential commands. Immediate 
commands Cause instantaneous changes to control variables and are implemented using a 
single process. A sequential command is only executed after the robot has completed the 
previous sequential command. Therefore, each sequential command requires a process 
pair. One process, called from the user program, stores the command in the instruction 
buffer. The other process executes the command and is called from the motion system 


control process. 
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Figure 13: Motion Control System 


The system’s second purpose is to control the robot’s movement. This is done 


by executing the motion system control process every 10 milliseconds. Each motion control 
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cycle begins by updating the current configuration using information obtained from the 
wheel system. Using motion control parameters and the new configuration, it then 
calculates the desired linear and rotational velocities needed to follow the current path 


element. These desired velocities are then sent back to the wheel system for execution. 


5. Wheel System 


As indicated by Figure 13, the wheel system is a component of the motion 
control system. Presented in Figure 14, the system’s primary function is to provide an 
interface between the motion control system and the dual axis controller. This interface was 
designed to eliminate knowledge about the robot’s architecture from the motion control 
system. Therefore, the wheel system can easily be replaced by another type of locomotion 


system. 
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Figure 14: Wheel System 


6. Sonar System 


As seen in Figure 15, the sonar system’s design is similar to the motion control 


System. It provides the MML sonaz command interface between the user program and the 
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Figure 15: Sonar System 


sonar board. Some commands control the boards operation by setting control variables, 
while others return sonar information to the user program. It also contains the sonar system 
control process. This process saves the current sonar returns that may be requested by the 
user program. It can also record the sonar returns by sending the data to the sonar trace 


system. 


7. Tracing Systems 
A trace system is a small sub-system used to record selected information. Shown 
in Figure 16, its interface consists of three parts. First, it contains control routines to 
initialize and enable/disable the system. The initialization routine sets the size of the 


logging buffer, while the enable routine sets the frequency that the data is logged. The 
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second part of the interface consists of the logging routines. These routines save the 
selected data after ensuring that the buffer is not full. The third component downloads the 
recorded information to the host Unix system. Only character data can be sent to the host 
system. Therefore, the download process must convert the data if necessary. 

There are two tracing system components in the new design. One 1s located in 
the motion control system and is used for debugging. The other 1s located in the sonar 


system. This component is used to log sonar return information. 
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Figure 16: Tracing Systems 


8. Terminal System 


The transfer of information between a processor and a device, such as a terminal, 
is time consuming. In real-time systems, it is unacceptable for a high priority system to wait 
for I/O completion, monopolizing the CPU. However, the ability to display information 
from these high priority systems is still desirable. 

The purpose of the terminal system is to provide any sub-system the capability 


to display information. As presented in Figure 17, the terminal system accomplishes this 
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objective using asynchronous interrupt driven output along with a set of I/O routines. This 
allows a high priority sub-system to continue executing by sending output information to 
the terminal system for processing. Input is not interrupt driven and should only be used by 


low priority routines. 
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Figure 17: Terminal System 


When an output routine is called, the routine converts the data into a character 
representation if necessary. It then stores the string of characters into an output buffer that 
uses a queue data structure. The routine then sends a null character directly to the terminal 
using Console output. After a character is sent to the terminal, an interrupt is generated by 
the hardware requesting another character. This interrupt activates the terminal output 
control process. The process de-queues the first character from the buffer and sends it to 
the terminal, which then generates another interrupt. This operation continues until the 


buffer is empty and the control process terminates the interrupt cycle. 
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V. YAMABICO SOFTWARE DEVELOPMENT ENVIRONMENT 


The current software development environment for the robot is restricted to a single 
Sun-3 workstation using SunOS 4.1.1. The reason is that this workstation shares the same 
processor architecture as the robot. The development cycle will improve when the robot’s 
CPU system is upgraded to a SPARC-4 processor due to more advanced development 
tools, FTP transfer capability and support for system I/O by the resident debugger. 

The Yamabico-11 does not use a commercial operating system. This means the user 
must perform some of the basic operating system functions through the resident debugger. 
First, the user must load the program into memory. Then control of the CPU must be 
transferred from the debugger program to the user’s program by placing the address of the 
first instruction to be executed into the program counter (pc). The user must also ensure the 
program returns control to the debugger when it has terminated. Preparations for these 


functions begin with the compilation phase. 


A. C COMPILERS 
The only compilers available for the current development environment are the Unix C 
Compiler and the GNU Project C Compiler. Both compilers are invoked through a 


compiler driver. 


1. Compiler Drivers 

As shown in Figure 18, a compiler driver is a program that creates executable 
code by sequentially calling the pre-processor, compiler, assembler and linker with the 
appropriately supplied parameters and files. The driver for the Unix C compiler is cc, while 
the driver for the GNU C compiler is gcc. When invoked, the driver passes the C source 
file to the pre-processor. Output from the pre-processor is then passed to the compiler. Each 
compiler is essentially a translator, translating the C source file into equivalent assembly 
code. The compiler driver then passes this assembly code to the assembler to create object 


code. Next the driver passes the assembler output to the linker to build executable code. The 
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Figure 18: Compiler Driver Process 
output from the linker is then stored in a file in the developer’s directory. This process can 
be verified by using compiler flags to display the compiler driver commands. These flags 


are -dryrun for cc and -v for gcc [CPG 88][Stallman 89]. 


2. Code Optimization 

Producing optimized object code is one of the goals of program development. 
Both compilers will optimize source code if the -O flag is used. However, this is not always 
desirable. There are instances where several values must be assigned to a variable or 
address in succession, as in initializing serial ports for example. If this optimization 
function is enabled, the compiler would eliminate all but the last assignment, resulting in 
an improperly initialized port. This can be verified by inspecting the assembly code 
translation produced when the flag -S is used with -O. The -S flag instructs the compiler 
driver to stop after translating the C source file into assembly code, storing the translation 
in a file. This file will be given the same name as the source code with the exception of a .s 
extension. Inspection of the assembly code will reveal that only the last assignment is 
retained. Therefore the developer must be careful in deciding which modules can be safely 
optimized by the compiler [CPG 88][Stallman 89]. 

Another way of optimizing code is by using ‘#define’ statements to declare 
constants instead of ‘const.’ Constants declared using ‘const’ are stored on the stack during 
run time. References are implicit, using the stack pointer and an offset. Using the ‘#define’ 


method causes the compiler’s pre-processor to substitute the actual value into the source 
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code prior to compilation. This is the equivalent to hard coding the values except that it 1s 


cleaner and easier to maintain/modify. 


3. Standard Libraries 

It is assumed that the generated executable code will be run on the platform used 
for development. Therefore each compiler uses and depends on its own set of standard 
libraries. These libraries are usually linked with the object code automatically by the 
compiler driver when creating the executable. However, some of the routines in these 
libraries make calls to the operating system. Since the robot does not have an operating 
system, these libraries should not be used. Standard C functions for input/output or file 
operations such as printf() should also not be used. Therefore, the compiler driver should 
be instructed to stop before the linking phase by using the -c flag. It will then save an object 
file with a .o extension for each C source file passed to the compiler driver. These files can 


then be used in an explicit call to linker without the libraries [CPG 88][Stallman 89]. 


4. CC vs.GCC 
There 1s a major difference between the two compilers. The Unix compiler is for 
source code developed using the K&R standard, a style of developing C source code 
designed by Brian Kernighan and Dennis Ritchie. This standard was developed prior to the 
ANSI standard adopted in 1989. The GNU compiler was designed for source code wnitten 
in ANSI C. The use of function prototypes for parameter checking is one of the major 


advantages the ANSI standard has over the K&R standard. 


5. Using GCC 


The GNU Project C Compiler does have some peculiarities that must be taken 
into consideration when developing programs for the robot. First, the compiler generates a 
callto _main() as the first statement in main(). Since gcc’s libraries are not linked with the 


object code, the result will be an undefined symbol error during linking. Therefore, the 
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developer must explicitly create a function called _ main() that returns void. The function 
definition should only include a simple return statement. 

The compiler may also insert calls to certain standard library routines, such as 
memcpy() and memset(). Since the libraries are not included during the linking phase, again 
the result will be an undefined symbol error. Therefore the user must either develop these 
functions or build a new library for the robot. The Unix ar command can be used to extract 
copies of these functions from the standard libraries and to build the new library [SRM 88]. 

Another precaution is associated with how the compiler generates storage for 
locally declared strings. Storage for strings declared within a function body is allocated 
immediately prior to the address of the function itself. Currently, this only effects the 
functions main() and user() since they must start at an absolute address. This is discussed 
later in this chapter. Therefore, the developer should not declare local strings in either 
function. 

Two flags that are helpful to use are -Wall and -Wpointer-arith. The -Wall flag 
instructs the compiler to issue common warning messages. For example messages will be 
displayed when variables are declared but not used, when functions are implicitly declared 
as returning an integer or when functions declared to return a non-void value but do not 
have a return statement in the function definintion. To receive warnings concerning the use 
of pointer arithmatic, use the -Wpointer-arith flag. This allows the developer to find 


accidental uses of pointer arithmatic [Stallman 89]. 


B. LINKING 


Each object file passed to the Unix linker (/d) consists of a text segment and a symbol 
table. The text segment contains the executable instructions while the symbol table contains 
the symbols (function names and global variables) that can be accessed by other modules. 
When multiple object files are used, the linker starts by appending the text segment of the 
second file to the first’s. The text segment of each additional object file is then appended 


in the order given to create one text segment. All symbols are also collected into one 
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symbol table [SRM 88]. Figure 19 shows an example object module created from the 


following command: /d main.o iosys.o Serial.o. 


main.o functions 


iosys.o functions 


Serial.o functions 


symbol table: includes 
global variable names 
and function names of 
main.o, i0sys.o and 
serial.o 





Figure 19: Example of Composite Object File 


After all object files have been processed, the linker must calculate the absolute 
address of each symbol in the symbol table. Addresses for functions are determined using 
the offset relative to the beginning of the program and the program’s starting location. 
Absolute address calculation for global variables is based on an offset from the next page 
boundary following the text segment. It then resolves all references to these symbols. 

After all references are resolved, the linker creates a load module. This module is a file 
of records used by the loader. These records contain either instructions or initialized global 
data along with the address in memory where they are to be located by the loader. All of 
the records containing text (instructions) are collected and placed prior to the records 


containing the initialized global data. 
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1. The Kernel Module 

Currently, all programs are divided into two load modules. One module is the 
kernel while the other contains the motion commands. The kernel contains main() and most 
of the code. As mentioned earlier, the user must know where the program execution begins. 
It is standard practice to load the kernel! at hexadecimal address 0x304000. Therefore, the 
first instruction in the function main() must be located at this address. To correctly resolve 
the symbolic references, the kernel’s starting location must be passed to the linker by using 
the -T 304000 flag [SRM 88]. 

Since the creation of the text segment is based on the order of object files passed 
to the linker, the object file containing the definition of main() must be the first file in that 
list. The order of the remaining files is unimportant. Additionally, since cc and gcc generate 
object code as defined in the C source file, main() must be the first function defined 1n its 
source file. Finally, if local strings are defined in main(), gcc will reserve memory storage 
prior to main(). Consequently, 0x304000 would be the address of the string instead of the 
first executable instruction. These last two points can be verified by using the -S flag and 


inspecting the resulting assembly code. For an example, see Figure 20. 


C Source Assembly Translation 


main(){ -ascii “Hello World\0” 
. even 


printf(“Hello World”); 


jbsr _ printf 





Figure 20: Effects of Compilation on Strings 
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2. The User Module 

The user module contains MML library function calls. Its construction is similar 
to the kernel with a few exceptions. First, the function User() is the primary function 
instead of main(). Second, the module is loaded at hexadecimal address 0x334000 vice 
0x304000. 

Since User() is called by the kernel, the kernel must know User()’s location. 
However, User() must also know the location of the functions in the kernel too. Making 
User() accessible to the kernel is accomplished by declaring pointer to a function returning 
void within the kernel, and initializing it to 0x334000. To provide access to the kernel’s 
functions, use the -A kernel flag to link the user module with kernel’s symbol table. One 
note of caution, the user module is now dependent on the kernel. Therefore any change to 


the kernel requires the user module to be re-linked. 


C. LOADING 


Before a program can be executed, it must be placed into random access memory by a 
loader. Each record in the load module contains instructions or initialized data information 
along with an address. The loader then loads the information from each record at the 
address specified. 

However, the loader for the robot is a simple program that essentially dumps the 
information sequentially from all records beginning at either 0x304000 or 0x334000. This 
does not interfere with the placement of the program instructions. But the global variable 
initialization data is erroneously placed at the end of the text segment instead of in the 
global variables. Therefore, global variables initialized at compile time actually contain 
unpredictable data when the program is loaded. The solution to this problem is to initialize 
all data at run-time, prior to executing the program. Figure 21 shows the robot’s memory 


framework after program loading. 
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0x304000 







kernel text segment 


kernel data segment 


user text segment 
o 
@ 





First page boundary 
after text segment 


0x334000 


Figure 21: Yamabico-11 Memory Map 


D. DEBUGGING TOOLS 


1. The Onboard Debugger 
As mentioned earlier, the CPU system includes the VME7920 Debugging 
Package. This monitor program is primarily used to load and execute programs on the 
robot. However, it can be used to debug programs. The developer can set breakpoints at a 
particular instruction, halting program execution. The value of variables or registers can be 
examined and modified. The debugger will even aid the developer by displaying machine 
code in assembly instruction format. The only requirement is knowledge of the 


instruction’s or variable’s memory location [VDP 86]. 
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2. The Unix nm Command 
To obtain the address of variables or functions from an executable file, the Unix 
command can be used. The nm command displays a files symbol table. For example, 
entering nm -n user will generate a listing containing each symbol (global variable or 
function name) in user’s symbol table along with its location in memory. The -n flag 
instructs the command to sort the list by memory location. This listing gives the developer 
the memory address locations needed to debug programs with the VME7920 Debugging 
Package [SRM 88]. 
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VI. SYSTEM IMPLEMENTATION AND TESTING 


ANSI C was the language chosen to implement the new design instead of the older 
K&R version used in MML-10 since it provides stronger type checking through prototypes. 
Since the two versions are similar, some of MML-10’s source code could be ported to the 
new design with minor modifications. ANSI C also enables a smooth transition of MML to 
an object oriented paradigm using C++. However, the only ANSI C compiler locally 


available for the robot’s software development environment is GNU’s gcc. 


A. IMPLEMENTATION TECHNIQUES 


1. Stability 

Producing a stable system is one of the primary objectives during the 
implementation phase. One method for producing a stable system is to use encapsulation. 
In C, this is accomplished by declaring global variables and local functions with the static 
specifier. These identifiers can be referenced from sources within the file but prevents 
external references. 

Reducing unnecessary pointers and pointer arithmetic is another way of 
producing a stable system. Generally, there are only two reasons to call a function. The first 
reason 1S to produce some action, such as displaying a value. In this case, actual values are 
passed as parameters and the function does not return a value. The second reason to call a 
function is to process information, producing one or more results. In this case, it 1s common 
to let the function assign the values directly by passing the addresses of these variables as 
parameters. However, this can lead to many programming errors. A better method for 
writing these functions is to use a return statement. To return more than a single value, a 


structure of values should be used. 
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2. Portability 


a. Required Assembly Code 

Each assembly instruction corresponds to a single machine instruction. It 
also produces hardware dependencies since each processor has its own assembly language. 
Therefore programming in assembly language is very difficult and should be avoided. 

One reason to use assembly language programming Is to increase the speed 
of program execution [Schildt 90]. Although MML is a real-time system with timing 
constraints, it is still a research project. Using assembly language to increase program 
execution simply adds complexity. 

The primary reason to use assembly language is to access specific hardware 
components or instructions that can not be accessed in any other manner [Schildt 90]. The 
status register and rte instruction are examples of such requirements when using the 
Motorola 68020 processor. Modification of the status register is handled by instructions 
used for special registers. The rte is used to return from an interrupt handling routine 


instead of the normal rts instruction. 


b. Handling Interrupts 


Setting up an interrupt mechanism requires two steps. First, an interrupt 
handler must be created. This routine is a basic shell that must be written in assembly 
language. The routine must begin by saving all of the CPU’s registers onto the stack. If a 
coprocessor is used, those registers must be saved as well. Then the routine must make a 
call to a C function that will control the real processing during the interrupt cycle. Once 
control is returned from the C function, the interrupt handler must restore each register to 
the value it contained prior to the interrupt. It is important to note that the registers must be 
restored in the reverse order as they were saved. Finally, the rte instruction is used to return 
from the interrupt. 

The second step in establishing an interrupt mechanism is to setup the CPU 


and interrupting hardware component. This is a two step process. First, the address of the 
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interrupt handler must be placed in the CPU’s vector table. Next, the index value of the 
table entry is passed to the interrupting device. Once these two steps are complete, the 


hardware device can be enabled. 


c. Absolute Addressing inC 
In C, each variable declared as a pointer stores a 32-bit memory address and 


can be assigned constant values between zero and 237.1. The amount of memory that a 
pointer references depends on the pointer’s declaration. For example, if a variable is 
declared as a character pointer, only a single byte, located at pointer’s value, will be 
referenced. But, if the variable is declared as an integer pointer, then four bytes will be 
referenced by the pointer, starting at the value of the pointer. 

The amount of memory that a pointer references can be temporarily 
changed. Using the cast operator, a pointer 1s changed to another pointer type. However, 
the change only lasts for that operation. Afterwards the pointer reverts back to a pointer of 


its declared form. 


3. Readability 


One technique for making a system more readable is to use descriptive names 
for functions, variables and constants. Abbreviations should never be used. When declaring 
a symbol, two conventions are generally used: separate multiple words with underscores or 
capitalize the first letter of each word in the symbol. The later convention is used 
throughout this implementation. Another naming convention utilized in this 
implementation is to declare or define constant symbols with all capital letters. 

Header files are used to declare the prototypes for each function in a module that 
can be accessed using external calls. This presents the file’s interface and should be well 
documented. The comments should describe what parameters are required and the expected 
results. A description of the algorithm is not needed. However, constants needed by a C file 


that may also be referenced by other files are included in its header file also. 
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Another technique to making a system more readable 1s to limit the size of the 
functions to one page. Techniques such as page breaks between functions and good 


descriptive comments should also be used. 


4. Backward Compatibility 

To maintain backward compatibility, the new system generates user and kernel 
object files. The kernel is still loaded at 0x304000 and user is loaded at 0x334000. The 
absolute address of user() is declared in motorola.asm.s as _user. To prevent the linking 
error of duplicate symbols, the actual name of the user function in user.c should be User() 
instead of user(). However, to transfer control to the user program, main() is still required 
to call user() instead of User(). 

To maintain compatibility with MML-10’s immediate command names, a 
translation header file was created. This is a file containing macros, used by a pre- 
processor, that replace the old command syntax with the new command syntax. A similar 
one should be created for other MML-10 commands so that previously created user 


programs can operate with the new system. 


B. SYSTEM IMPLEMENTATION 


Due to the scope of the design, it was decided that only acore portion of the new design 
would be implemented, starting with the CPU system. The system described in this section 
consists of files with .c, .h, and .s extensions. Those files ending with a .c are C source files. 
These files contain function definitions and encapsulated data structures. The .h files are 
the header files, while the files ending with a .s are written in assembly language. With the 
exception of main.c, each C and assembly file has an associated header file. All files are 
presented in the Appendices and were developed as part of this study except for a few 
routines found wheels.c and motorola.asm.s. These 1outines were ported from MML-10. 
However, the system does make use of some utility functions, such as memory 


management and math routines that were developed in earlier projects. 


41 


1. The CPU System 

The CPU system was implemented first since it is required to initialize all other 
systems. Appendix A contains the source code for this system. It contains the files: 
definitions.h, main.c, System.h, system.c, motorola.h, motorola.asm.s, serial.h, and serial.c. 

The definitions.h file contains the system’s standard type declarations. Some of 
the new data types refer to constructs used by the motion system. Other declarations define 
standard assembly language data types, such as BYTE and WORD. These definitions make 
the code easier to follow. 

The file main.c only contains main() to prevent the linking problem discussed 
earlier. This is the function that gets loaded at Ox304000. It initializes the sub-systems and 
then calls user(), the user program. After the user program is finished, rexit() 1s called to 
return control back to the resident debugger. 

The Initialization routines called by main() are located in system.c. They are used 
to setup the interrupt mechanisms described earlier. Also included in this file are the some 
system functions for enabling and disabling interrupts and the __main() function required 
by the gcc compiler. 

The motorola.asm.s module contains all of MML’s required assembly code. The 
interrupt handling shells described above are located in this module. Also defined here is a 
routine to change the CPU’s interrupt priority level. The rexit() routine is located in this 
module since it uses a sequence of instructions that are difficult to duplicate using a high 
level language. The address of user() 1s also set in this file. 

The serial.c module contains the serial system. As mentioned earlier, this is a 
component of the CPU system. It serves as an interface between the serial boards and the 
other sub-systems. The previously discussed technique for referencing absolute memory 


locations 1s evident in this module. 
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2. The Terminal System 

The terminal system was chosen as the next system to implement as this would 
aid in debugging other systems. The files :osys.h and losys.c contain the system’s source 
code and are located in Appendix B. The functions defined in losys.c provide input and 
output for string, integer and real number data types. 

Some of the functions depend on conversion routines. Some routines convert 
strings to integer or real values. Others convert integer and real values to strings. Notice 
how the global data is encapsulated by using the static specifier. This prevents routines 


outside this module from accessing these variables. 


3. The Motion Control System 
Appendix C contains the files: motion.h, motion.c, motiontrace.h motiontrace.c, 
wheels.h, and wheels.c. These files define the motion control system. motion.c defines the 
functions that support odometry and velocity control. The wheel system component is 
defined in the wheels.c module while motiontrace.c defines motion’s tracing system 
component. The tracing system depends on memory management routines. The motion 


control rules required for path following are not implemented in this core system. 


4. The User Program 


A sample user.c is included in Appendix D along with its user.h. This module 
shows how to use the core system. Another file associated with the user system is 
compatability.h. This file provides the translation layer for older user programs developed 


for MML-10. 


C. SYSTEM TESTING 


Since the system was incrementally developed, testing was conducted at each system 


modification. This section describes the testing techniques that proved most helpful. 
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1. Using an External Power Supply 
The robot’s battery power was often drained due to software testing by several 
people. System testing was aided by an AC power supply. To test the motion control 
system, the robot was placed on wooden blocks to prevent movement. The power supply 
provided sufficient power when testing the CPU and terminal systems. However, it was 
discovered that the generator did not produce enough power to drive the wheels above 
moderate speeds. Fast wheel movements caused the CPU to shut down due to insufficient 


power. So stationary testing was conducted at slow speeds. 


2. Output from Interrupt Handlers 

Displaying information to the terminal from the motion control system required 
special handling. The problem was that the information would be sent to the terminal 
system every 10 milliseconds, causing the output buffer to overflow. The solution was to 
send the messages at a lower frequency. This was accomplished by using the LoopTest 
variable in motion.c as a counter. Every motion control cycle incremented the LoopTest. 
When the counter reached the frequency value of 100, it would be reset to zero. All output 
messages were then placed in a conditional block that checked the value of LoopTest. Only 
when it was zero would the message be sent to the terminal system. The result is that the 


message would be sent to the terminal every second instead of every 10 milliseconds. 


3. Measurements 

The first part of the motion control system to be implemented was the wheel 
system component. Manual calculations were required to verify the operation of this 
system. To verify the change in distance and orientation information, the robot was 
manually pushed along a path with known distance. Several trials were performed using 
paths of different lengths. When the robot’s results were compared to the path lengths 
measured with a measuring tape, the differences were consistently within one centimeter. 

To verify the velocity inputs to the wheel system, the tracing component is used. 


Figure 22 is an sample of results obtained by changing the robot’s commanded velocity 





Figure 22: Velocity Control Results 


from zero to 20 cm/s. The results show that the actual velocity is one to two centimeters per 
second faster than the commanded velocity. This difference was found at all speeds tested 
and is the result of a lookup table used to calculate pwm values. This table is tailored for 


this specific system and requires further testing to correct the discrepancies. 
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Vil. CONCLUSIONS 


A. RESULTS 

The end product is a solid core system, capable of serving as a foundation for further 
MML research. The system is implemented using a standardized language. Hardware 
dependencies are localized and the required assembly code is reduced to a single module. 
This results in a system that is very portable. Each module is provided with a clean interface 
making the system very modular. With limited pointer references and data encapsulation 
the system is also very stable. The result is a system that is very readable, easy to maintain 
and easy to modify. 

One of the underlying themes presented in this study is pointer reduction. This is not 
to imply an unfamiliarity with the C language or the use of pointers. In fact the author is 
aware of the value of using pointers and has been programming with pointers in the C 
language since 1985. However, many of the researchers on the MML project do not possess 
the same experience level. This has resulted in improper pointer usage, thereby increasing 
program complexity and decreasing stability. This is the primary reason for recommending 


the reduction pointer usage. 


B. RECOMMENDATIONS 

The next step for this new system is to complete the motion control system. This 
requires the implementing of the motion control rules to provide path following. It also 
requires the addition of the sequential commands and the instruction queue. 

Following the completion of the motion control system, the sonar system should be 
implemented. This will involve writing the interrupt handler shell and control process. The 
assembly code should also be converted to ensure portability. 

Another potential project is to develop a simulator based on the new system. Due to 
the system’s modularity, this should only require the simulation of the interrupt 


mechanisms and the hardware dependent code. 
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APPENDIX A 


A. DEFINITIONS.H 
eek dea were eT ee es ee rece ee eee 


Author(s): Scott Book 

Project: Yamabico Robot Control System 

Date: December 8, 1993 

Revised: March 4, 1994 

File Name: definitions.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains standard definitions and data type 
declarations used throughout the reset of the MML system. 


ieee or Oe ee ee ae etre Sheer Te ne Cee ene eee | 


#ifndef DEFINITIONS_H 


#define _ DEFINITIONS _H 


#define MOTION_CONTROL_CYCLE 0.01 


#define MAX_REAL_PRECISION 15 
#define MAX_INTEGER_DIGITS 19 


typedef enum {FALSE = 0, TRUE} BOOLEAN; 


typedef unsigned char BYTE; 
typedef unsigned short WORD; 
typedef unsigned long LONG; 
typedef unsigned long* ADDRESS; 


typedef struct{ 
double Linear; 
double Rotational; 
} VELOCITY; 


typedef struct{ 
double XPosition; 
double YPosition; 
} POINT; 


typedef struct; 
POINT Position; 
double Orientation; 
double Kappa; 
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} CONFIGURATION: 


#endif 


MAIN.C 
[PES OA Ae ee eee Nee SNE Se Se eee cee neg actaaieiaaieeig meg ekccecie eee ian 


Author(s): Scott Book 

Project: _Yamabico Robot Control System 

Date: December 8, 1993 

Revised: March 4, 1994 

File Name: main.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains main(). Its purpose is to initialize all 
sub-systems and then pass control to user(). Once user() is 
complete, the routine returns control to the resident debugger. 


ill ak lla aii a li did ii ta all che ad lll A olla EI) 


#include “definitions.h” 
#include “motorola.h” 
#include “system.h” 
#include “memsys.h” 
#include “iosys.h” 
#include “senal.h” 
#include “motion.h” 
#include “user.h” 
void user(); 
void main(){ 
Cpusysinitialize(); 
ResetSenalBoards(); 
SetinterruptPrionty(7); 
InitializeMemSys(); 


InitializeConsole(); 
InitializeHost(); 


loSysinitialize(); 
MotionSyslinitialize(); 


SetinterruptPriority(0); 


user(); 
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rexit(); 








C. SYSTEM.H 


eee paths age ay tage erg He teres estes ener ee roe eT ie 


Author(s): Scott Book 

Project: _Yamabico Robot Control System 

Date: December 8, 1993 

Revised: March 4, 1994 

File Name: system.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the prototypes for routines that are hardware 
dependent. 


RII el a a adil La aa lal alice ol all 


#ifndef_ SYSTEM_H 


#define SYSTEM_H 


trey ees en ee re ee ere ene ern mes ome ee ere 


Function __main() is an empty function. It is required if gcc is used since 
gcc will insert a call to __ main() within main(). 


Rg ote ee ee Gee ee ee eer ene me Ge ey 
* 
void  main(): 
Ss g 


ee Ce cae ee ene ne eee ee see Te een re ets Se eo eee 


Function CpuSysinitialize() is used to set up any requirements that are 
specific to the Motorola 68020 CPU. 


De ge re re ee ee ee eee eC Ree ery Cer ere nce Mae en f 
s ° es s 
void CouSysinitialize(); 
i] 
pegeen ce en ee ere ee ee ce ceen cee eee ee 


Function Enablelnterrupts() turns on interrupt servicing. 


IB ak a hl aaa a a ll lalla laa al alalalil | 


void Enablelnterrupts(); 





ee ck ee ee eee ee eee a Se ee eet ee ee ee 


Function Disableinterrupts() turns off interrupt servicing. 


Oe Gage Se te ee cee eee ee eee eel eye een 
e e 
void Disablelnterrupts(); 


BPO ODN N A athe alt a  i a liaal id laealaliaalh ak e ataaali ala l 


Function SetConsolelntMechanism() establishes the console’s interrupt 
driven output mechanism. 


i sete weer cae Se eee ee eee ene ee nee hee ee eee ee ey 


void SetConsolelntMechanism(); 
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| hahahahahahaha lal hesi aad ec hie ale hich Ah hh ad olE ah he R ASIP I ANDES IE 


Function SetMotionIntMechanism() establishes the synchronous interrupt 
mechanism required for motion control. 


aaa cca lalla alice clea al SALES DAS ALR RAL Rial AK AMR be hE LBA AILS AL | 


void SetMotionIntMechanism(); 


#endif 


SYSTEM.C 
halal alla aaah iki hdd d du! he halaadesalalalicigte iii A PALA Doh fol Lh RabdabedgDafede BRN EA ETI ES 


Author(s): Scott Book 

Project: _Yamabico Robot Control System 

Date: December 8, 1993 

Revised: March 4, 1994 

File Name: system.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the required cpu specific routines. The file 
is seperated into two sections: private and public. The private 
section contains the encapsulated data while the public section 
contains the routines available to other systems/modules. 


Be eee eee re er Ne ree te er a age tee reat a 


#include “definitions.h” 
#include “motorola.h” 
#include “serial.h” 
#include “system.h” 


Static int SavedinterruptMask; 
static BOOLEAN InterruptsOn; 


| hadnt!" lille toil ich ai i a halla a aah AO 


Routine __—s main is required when using the ‘gcc’ compiler. This is because the 
compiler inserts a call to this routine at the beginning of the main function 

defined for the program. This is normally taken care of by linking in the 

bootstrap object modules, however these are not added to a program that 
operates without an operating system such as the mml program. Therefore, since 
this routine is called, the only requirement is for this routine to simply 

return back to the main program. 


ltteer didlo ah al hh hhh DD AA IIASA SS 


void __main(){ 
retum; 


} 
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ee ea err Ss ge te age te ee ean ngeae sees oe ee 


Function CpuSyslinitialize() is used to set up any requirements that are 
spec :t:¢ to the Motorola 68020 CPU. 


eee ee eam age ene ee a eee eee meres 


void CpuSyslnitialize(){ 
ADDRESS VectorTableBaseAddress = GetVectorBase(); 


rece gae i ee ee ge es ee ee 


* Use a blank interrupt handler to recover from unexpected exceptions 
* such as when the status word is set to a specific value in function 

* SetinterruptPriority. So enter the address of that interrupt handling 

* routine into the 255th entry of the exception/interrupt vector table. 


ee pg cena eee ee eee en eee nh ee reese Ney 


*(VectorTableBaseAddress + 255) = (LONG)Unexpected; 


InterruptsOn = TRUE; 
SavedinterruptMask = 0; 


ieee tigie ex ger gieg cee as ont ce re ee we oe See ine cre ee ee ese sa eee 


Function Enablelnterrupts() allows other systems to reset the interrupt mask 
to the previously stored value. 


ey Cage ere ne en er en eg Bre ge wee eee eee ree ray 


void Enablelnterrupts(){ 
if(InterruptsOn == FALSE){ 
InterruptsOn = TRUE; 
SetinterruptPriority(SavedinterruptMask); 
} 
} 


Pana ee ge or ee emer eee ek eter ee Seer nae er 


Function Disablelnterrupts() allows other systems disable all interrupt 
servicing. The current interrupt mask is saved so that it can be restored 
later. 


Dis Se er ne Pee orene Ter eee Ney 


void Disablelnterrupts(){ 
it(InterruptsOn == TRUE){ 
SavedinterruptMask = SetInterruptPriority(7); 
InterruptsOn = FALSE; 
} 


} 
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Hee eee eee eee ee ae Oe mam er eine ale miglen ee oot Gand ny gn oe eae 


Function SetConsolelntMechanism() establishes the console’s interrupt 
driven output mechanism. 


slat! al aalalalalal alalaelabalaialaialaicbalaialsleiialais ictal abel lata algal aiclabala) spabalicfaleAabelplahha\tal-tet hot AAS I hihi Hy) 


void SetConsolelIntMechanism()}{ 
ADDRESS VectorTableBaseAddress = GetVectorBase(); 


Disablelnterrupts(); 

/* Enter the address of the interrupt handling routine into the 65th */ 
/* entry of the exception/interrupt vector table of the CPU. a} 
*(VectorTableBaseAddress + 65) = (LONG)loSysHandler; 
SetLatch(Oxffff0031,65); 


Enablelnterrupts(); 
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Function SetMotionIntMechanism() establishes the synchronous interrupt 
mechanism required for motion control. 
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void SetMotionIntMechanism(){ 
ADDRESS VectorTableBaseAddress = GetVectorBase(); 


/ Formula: counter_value = (4e06MH / 16 frquency divider) * Interrupt_Interval */ 
ex: +(4e06/16) “ 0.01 seconds = 2500 " 
SetTimer(TIMERADDRESS_1,5,0x1c61,2500,2500); 

Disablelnterrupts(); 

/* Enter the address of the interrupt handling routine into the 64th */ 

/* entry of the exception/interrupt vector table of the CPU. °) 
*(VectorTableBaseAddress + 64) = (LONG)MotionSysHandler; 
SetLatch(Oxffff0 141,64); 


Enablelnterrupts(); 
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E. MOTOROLA.H 
(en ee ceer ye ne tn ee eee ee tee ere ee ee ee oe nee 


Author(s): Scott Book 

Project: _Yamabico Robot Contro! System 

Date: December 8, 1993 

Revised: March 3, 1994 

File Name: motorola.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the prototypes to the required assembly 
language routines. 


Giaiedaabai dee giceactnige ery er een cece eine eee gee en re we ee gene ume a eee ee ee ey 


#ifndef MOTOROLA_H 
#define _MOTOROLA_H 


#include “definitions.h” 


ees ee eee rere settee ene eens eee nye e a ewe a 


Function rexit() is used to safely return to the debugging routine used on the 
Yamabico’s processor. It is a required call in order to run the kernel again 
without having to reload tt. 


A ec ere yer eo One cee Cane tants Pane a hae teen ee een) ay 


void rexit(); 


ide cme 2 Ge sete ae Se Pe gage een ce oe coe rae er wae aE. 


Function GetVectorBase() returns the starting address of the interrupt vectors 
to the calling routine. This is needed in order to calculate the positions to 
place the interrupt handler addresses. 
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ADDRESS GetVectorBase(); 
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Function SetinterruptPriority() sets the interrupt priority level to the value 
passed in as a parameter, while returning the old priority level to the 
calling function. The return value can then be used to reset the prionty 
level at a later call. The parameter must be a 4-byte integer, and the value 
must be in the range 0-7. If it is out of range, a -1 is returned to indicate 
an error. 
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int SetinterruptPriority(int); 
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Interrupt handler Unexpected is used to handle the unexpected exceptions 
raised dunng execution of the program. Explicitly changing the status word 
is an example of such an exception. 
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void Unexpected(); 


[CAPEASERERRERS TREN E ET ORR TN ON eee ee ee ae Te ot ie ee ee 


Interrupt handler _loSysHandler is used to handle the interrupts from the 
serial port that is set up to handle the console I/O. It is really a shell 
routine needed to call the C function that does the real work. 
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void loSysHandler(); 


[SEAR ERERR EERE REESE CeO ea" “ie (Ge oO ee Cees oe 


Interrupt handler _MotionSysHandler is used to handle the interrupts from a 
timer on the serial board set for synchronous interrupts. 
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void MotionSysHandler(); 


#endif 


MOTOROLA.ASM.S 


# Author(s): Scott Book 

# Project: _Yamabico Robot Control System 

# Date: December 8, 1993 

# Revised: March 3, 1994 

# File Name: motorola.asm.s 

# Environment: Sun-3 assembler for the motorola 68020 processor 

# Description: This file contains the only required assembly lanaguage for the 





# MML system. The main purpose of this file is to define the 
# system routines that can not be defined using a higher level 
# language. 
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# The following declaration is necessary when using cc with the -f68881 argument 
# since the corresponding library is not linked in. It can be eliminated if gcc 
# is used. 

comm  f68881_used, 4 


# Defines the address of user(). 
.globl _user 
_user= 0x334000 


.globl _rexit 

.globl _GetVectorBase 
.globl _SetinterruptPriority 
.globl _Unexpected 
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.globl _loSysHandler 
.globl _MotionSysHandler 
.data 

text 


OA OT EO 
# Routine _rexit is used to safely return to the debugging routine used on the 

# Yamabico’s processor. 

Ca a ea 


even 
_rexit: 

trap #15 

.word 0x0063 

rts 


SHER RHRRHRRRHER ERR ERE E pe oe ee te Re tere Sp 
# Routine _GetVectorBase returns the starting address of the interrupt vectors 
# to the calling routine. This is only needed by higher level languages that 





even 
_GetVectorBase: 
link a6,#0 | When entering an assembly subroutine, use the 
moveml #0,sp@ | link command to preserve the previous address 
| inthe stack pointer. This makes parameter 
| passing and clean up simpler. The previous 
| contents of a6 are then pushed onto the stack. 
clrl do | Return the starting address of the vector 
move vbr,d0 | interrupt table to the calling routine. 
unlk a6 | If the link command was used, ensure that the 
rts | unlk command is also used to restore the 


| previous contents of both the stack pointer 
| and a6 when the subroutine was entered. 


FEAT RARE RH Oe Rg toe Be ht ne 
# Routine _SetinterruptPnority sets the interrupt priority level to the value 

# passed in as a parameter, while returning the old priority level to the 

# calling function. The return value can then be used to reset the priority 

# level at a later call. The parameter must be a 4-byte integer, and the value 

# must be in the range 0-7. If it is out of range, a -1 is returned to indicate 

# an error. 





BRT TS Be A boa AER 
even 

_SetlnterruptPriority: 
link a6,#0 | When entering an assembly subroutine, use the 
moveml d1-d2,sp@- | link command to preserve the previous address 


| inthe stack pointer. This makes parameter 


= 


L10:movl 


L20:moveml 
unlk 
rts 


a6@(8),d2 


#7,d2 
L10 
#0,d2 
L10 
#8,d2 


sr,d0 
d0,d1 
#0OxFOFF,d1 
d2,d1 
di,sr 


#0x0700,d0 
#8,d0 

L20 

#-1,d0 


sp@+,d1-d2 
a6 


| passing and clean up simpler. The previous 

| contents of a6 are then pushed onto the stack. 
| Also save and clear any registers that might 

| be used in the routine. 


| Get the first parameter. 


| Ensure that the parameter is between 0 and 7. 
| If it’s not, then branch to the error area. If 

| it is, then place the value in the correspond- 

| ing interrupt priority area of the status 

| register 


| Get the current status word and use it to 
| construct the new status word with the 

| interrupt priority greater than or equal to 
| the previous interrupt priority. All other 

| bits remain unchanged. 


| Finally, get the value of the previous 
| interrupt priority by selecting the priority 
| bits and then return the value in register dO. 


| An error has occured, so return a negative 
| value. 


| Restore the registers that were saved on 

| entry. If the link command was used, ensure 

| that the unlk command is also used to restore 
| the previous contents of both the stack 

| pointer and a6 when the subroutine was 

| entered. 





# Interrupt nanele? “lidHexeocteak is iisaa to sipanals the enexpecied exceptions 
# raised during execution of the program. Explicitly changing the status word 
# is an example of such an ex cen 
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even 


_Unexpected: 


rte 
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# waintenrtibt pales “laSuskianalente is used to mnanale the nang vii the 

# serial port that is set up to handle the console I/O. It is really a shell 

# routine needed to call the C function that does the real work. All of the 

# system registers must be pushed onto the stack prior to calling the C function 
# since the compiler may or may not save the contents of the registers prior to 
# use, thus possibly corrupting any data in the functions that were interrupted. 
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# The restoration of the register must be done in reverse order! 


FABRA HHH Et 


even 
_loSysHandler: 

link a6,#-184 

fsave a6@(-184) 

fmovemx fp0-fp7,sp@- 

fmovel fpcr,sp@- 

fmovel  fpsr,sp@- 

fmovel _ fpiar,sp@- 

movem! d0-d7/a0-a5,sp@- 

movel #-65529,a0 

moveb a0@,d0 

jsr _loSysControl 

moveml sp@+,d0-d7/aQ-a5 

fmovel sp@+,fpiar 

fmovel sp@+,fpsr 

fmovel sp@+,fpcr 

fmovemx sp@+,fp0-fp7 

frestore a6@(-184) 

unlk a6 

rte 
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| When entering an assembly subroutine, use the 
| lnk command to preserve the previous address 
| inthe stack pointer. This makes parameter 

| passing and clean up simpler. The previous 

| contents of a6 are then pushed onto the stack. 

| Then save all of the system registers. 


| Clear the B control/status register of 
| port 2 for console output (as per example 
| in serial pot manual). 


| Call the C function that is the real work 
| horse of the interrupt handler 


| Restore system registers in reverse order. 

| If the link command was used, ensure that the 
| unlk command is also used to restore the 

| previous contents of both the stack pointer 

| and a6 when the subroutine was entered. 


# Interrupt handler _MotionSysHandler is used to handle the interrupts from the 
# serial board timer that is set up to generate synchronous interrupts for motion 
# control. It is really a shell routine needed to call the C function that does 

# the real work. All of the system registers must be pushed onto the stack prior 
# to calling the C function since the compiler may or may not save the contents 
# of the registers prior to use, thus possibly corrupting any data in the 

# functions that were interrupted. The restoration of the register must be done 
# in reverse orderll 


2@eGePeteSseSewee 
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even 
_MotionSysHandler: 

link a6,#-184 

fsave a6@(-184) 

fmovemx fp0-fp7,sp@- 

fmovel  fpcr,sp@- 

fmovel _ fpsr,sp@- 

fmovel _ fpiar,spo@- 

moveml d0-d7/a0-a5,sp@- 

jsr _MotionSysControl 


s@eGeGeu Gee eme e em@e . 
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| When entering an assembly subroutine, use the 
| link command to preserve the previous address 
| in the stack pointer. This makes parameter 

| passing and clean up simpler. The previous 

| contents of a6 are then pushed onto the stack. 

| Then save all of the system registers. 


| Call the C function that is the real work 
| horse of the interrupt handler 


a7 
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moveml sp@+,d0-d7/a0-a5 
fmovel sp@+,fpiar 

fmovel sp@+,fpsr 

fmovel sp@+,fpcer 
fmovemx sp@+,fp0-fp7 
frestore a6@(-184) 

unlk a6 

rte 


G. SERIAL.H 


| Restore system registers in reverse order. 

| If the link command was used, ensure that the 
| unlk command is also used to restore the 

| previous contents of both the stack pointer 

| and a6 when the subroutine was entered. 
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Author(s): Scott Book 


Project: | Yamabico Robot Control System 


Date: December 8, 1993 
Revised: March 2, 1994 
File Name: serial.h 


Environment: GCC ANSI C compiler for the motorola 68020 processor 
Description: This file contains the prototypes/interface for the available 
serial system for the VME8300 Quad Serial Port 


Board. 
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#ifndef__ SERIAL_H 
#define _SERIAL_H 


#include “definitions.h” 


#define TIMERADDRESS_0O Oxffff0011 
#define TIMERADDRESS_1 Oxffff0111 


#define CONSOLE Oxffff0001 
#define HOST Oxffff0005 
#define YSB1 Oxffff0121 
#define YSB2 Oxffff0125 


| ais haaalaaalanelileelinhaechalie !So fh aliha igh SSI ahi aig! Nh aad a heli lilo ie Ech 


Function ResetSerialBoards() resets both VME8300 Quad Serial Port Boards. 
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void ResetSerialBoards(); 
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Function InitializeConsole() prepares the port that connects the onboard 


console with the robot for I/O. 
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void InitializeConsole(); 


[ONO ID Ra ee Baal aS i la Bel ll lh alll aA alii! lil a Raa alll lal llalilial anid talialiiia 


Function InitializeHost() sets up the port that connects the Unix work- 
Station with the robot to be used for object code and data transfers. 


ie Mo a ety er ee ee ne ot ee eT 


void InitializeHost(); 


Parte Sora gc gees ease pe ee ee ete a eer as ae gl eal get 


Function InitializePort() establishes the communications of the given Port 
to the parameters given. Baud can be assigned standard communication rates 
300, 1200, 2400, 4800, 9600, or 19200. HandShaking, TxInterrupt, and 
RxInterrupt are used as ON/OFF flags. An assigned value of zero will disable 
these options, while any non-zero value will enable them. 
seeeteeeneneneneneneeesneneneneceseensnereneenseseeneneceseenenecenesnsenenenens/ 
void InitializePort(LONG Port, 
LONG Baud, 
LONG HandShaking, 
LONG Txinterrupt, 
LONG RxInterrupt); 


ne eo ert cn eee Cnet oe ee teeta ne renee were ee 


Function SetTimer sets and starts a particular timer to generate interrupts 
ste sun ae susan suneneusnnsneneenannenseeneneeeneeneneceneneneeeeneueen sen annense 
void SetTimer(LONG TimerAddress, 

LONG CounterNumber, 
LONG ModeCmd, 
LONG LoadCmd, 
LONG HoldC md); 


Rae ee cae oe Ge ee ee eerie ee ee eee ee ee age ee mor ee 


Function SetLatch() passes to the specified latch the index value of the 
vector table entry containing the interrupt handlers address to the serial 
board. 


een: (0.57. ee Ge ee ee ne ee Geren seers Serene een 


void SetLatch(LONG LatchAddress, BYTE VectorNumber); 


Pinna Stee ee te oe eS ats oe ce mae ee ee ne cen eee nee 


Function PutConsole() initiates the printing of the single character Source 
to the console. 


i ieReneaCart sey eee ee ne ee ee eee ney 


void PutConsole(char Source); 
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Function PutHost() sends a single character is the port connected to the host 
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workstation. 
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void PutHost(char Source); 


[PPPRESERERRRE EE EREEEETER CONT OTN OTe e ae aes | ee 


Function GetConsole() returns a single character typed from the console. 
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char GetConsole(); 
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Function Terminate ConsoleOutput() signals the console port that the interrupt 
generated by the port has been serviced. 
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void TerminateConsoleOutput(); 
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Function TerminateConsolelntCycle() tells the console port to terminate all 
unserviced interrupts. 
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void TerminateConsolelntCycle(); 


#endif 


SERIAL.C 


Deen eee eee ee ee mee te ee ae ae me eet wy epee Nl 


Author(s): Scott Book 

Project: Yamabico Robot Control System 

Date: December 8, 1993 

Revised: March 2, 1994 

File Name: serial.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the I/O routines necessary to interface 
between the VME8300 Quad Serial Port Board and the functions of 
the I/O System. The details of hardware setup and initialization 
followed the examples in the VME8300 Quad Serial Port Board 
Users Manual. The file is seperated into two section. The first 
is the private section containing the encapsulated data and 
functions. The second section is the public section. This 
section defines the interface routines. 

WARNINGII!This system must not be optimized when it is compiled. There are 
places where it is necessary to write several values to the same 
address in succession. If an optimization technique is used, the 
compiler could discard all but the last write. This would cause 
the serial board to be improperly setup. 
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#include “definitions.h” 
#include “system.h” 
#include “motorola.h” 
#include “iosys.h” 
#include “serial.h” 


ee en ee ie eee ete ONS eee ee eee ee eee rer ose 


PRIVATE SECTION 


The following section defines the encapsulated definitions, data structures 
and prototypes used in the system. 


eee aaa ene eS ware ey ore ee eee eS eee a eee eye ne ete eee ety 


#define ARM 0x60 
#define DISARM Oxc0O 
#define LOAD 0x40 


#define CONTROLREGISTER_0O 0 
#define CONTROLREGISTER_1 1 
#define CONTROLREGISTER_2 2 
#define CONTROLREGISTER_3 3 
#define CONTROLREGISTER_4 4 
#define CONTROLREGISTER_5 5 
#define CONTROLREGISTER_6 6 
#define CONTROLREGISTER_7 7 


[RR al dl la aa ha i a a a alle 


The following static function declarations are the prototypes for the 
encapsulated functions. 
eine Pode a aes wife re ee pe gee ee eee ee 
static void SetDataTransfer(LONG, LONG, LONG, LONG); 
static void PutB(char Data, int MemoryAddress); 


ade ogee ik eee eS een ee ON ee cert Sn ee teehee e Keene wee ene ee 


Function SetDataTransfer() establishes the communications for a given Port 
by setting the appropriate bits in each of the seven control registers for 

that Port. Details on the values sent to the command and data registers can 
be found in the VME8300 Quad Serial Board Users Manual. 


en vane amee: CCT ee fee te ee Cn eet eee een ey 
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Static void SetDataTransfer(LONG Port, 
LONG HandShaking, 
LONG Txinterrupt, 
LONG Rxinterrupt) 


BYTE* CmdRegister = (BYTE*)(Port + 2); 


*“CmdRegister = CONTROLREGISTER_O; 
*“CmdRegister = 0x18; 


*“CmdRegister = CONTROLREGISTER_2; 
*“CmdRegister = 0x00; 


*CmdRegister = CONTROLREGISTER_4; 
*“CmdRegister = 0x44; 


*“CmdRegister = CONTROLREGISTER_1; 
*“CmdRegister = 0x04 | TxInterrupt | Rxinterrupt; 


*“CmdRegister = CONTROLREGISTER_3; 
*CmdRegister = 0xc1 | HandShaking; 


*“CmdRegister = CONTROLREGISTER_5; 
“CmdRegister = 0xe8; 


*“CmdRegister = CONTROLREGISTER_6; 
“CmdRegister = 0x00; 


*“CmdRegister = CONTROLREGISTER_7; 
“CmdRegister = 0x00; 


| aaah iaa hala laaialiaialialaa cl oF eh BA hh T SAB AAL AIDS LALA hahsholoh heh! Nh hah Reliied thd  Plked 


Function PutB() displays the character in Data on the console. It does 

this by polling the command/status register to ensure that the transmitter 

buffer is empty and ready to receive the character. The delays (the for loops) 

are required to keep the data from overwhelming the port and becoming garbage. 
NOTE: 

The function required the ‘for loop as a timing delay. Without this delay, 

some of the characters sent to the console were lost. The reason behind this 

data loss was not determined. 
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Static void PutB(char Data, int MemoryAddress){ 
BYTE* OutputData = (BYTE*)MemoryAddress; 
BYTE* OutputCmd = (BYTE*)(MemoryAddress + 2); 
const int DelayTime = 0x300; 
int Wait; 
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*“OutputCmd = 0; 
for(Wait=0; Wait < DelayTime; Wait++) 


while((*OutputCmd & 0x04) == 0) 


*“OutputData = Data; 


if(MemoryAddress == CONSOLE && Data == ‘\n’){ 
for(Wait=0; Wait < DelayTime; Wait++) 


while((*OutputCmd & 0x04) == 0) 


*OutputData = ‘\r’; 
} 
} 
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PUBLIC SECTION 


The following section defines the functions that provide access to the 
serial sub-system. 
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Function ResetSerialBoards() resets both VME8300 Quad Serial! Port Boards. 


eae fe eee ge eg ee ee ee eee meee Mek Te oy Pee ee ae 


void ResetSerialBoards(){ 
BYTE* Timer; 
int Wait; 


Timer = (BYTE*)(TIMERADDRESS_0 + 2); 
“Timer = Oxff; 


Timer = (BYTE*)(TIMERADDRESS_1 + 2); 
“Timer = Oxff: 


for(Wait=0; Wait<0x100; Wait++) 
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Function Initialize Console() prepares the port that connects the onboard 
console with the robot for I/O. It also establishes the mechanism for 
interrupt driven output. 


Fee ER eee eRe En re tn nn oem aided ee ol epeaed ete eeaia) 


void InitializeConsole(){ 
InitializePort(CONSOLE,9600,0,1,0); 
SetConsolelntMechanism(); 
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Function InitializeHost() sets up the port that connects the Unix work- 
Station with the robot to be used for object code and data transfers. 


ahah laa ie ct al dhl e's ina al ahabalali glial ela a Sh adc hla PSAP) 


void InitializeHost(){ 
InitializePort(HOST,19200,0,0,0); 
} 


| alma hah aaah alae al al cil geal lh 


Function InitializePort() establishes the communications of the given Port 

to the parameters given. Baud can be assigned standard communication rates 
300, 1200, 2400, 4800, 9600, or 19200. HandShaking, TxInterrupt, and 
Rxinterrupt are used as ON/OFF flags. An assigned value of zero will disable 
these options, while any non-zero value will enable them. 
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void InitializePort(LONG Port, 
LONG Baud, 
LONG HandShaking, 
LONG Txinterrupt, 
LONG Rxinterrupt) 


LONG taddr; 
LONG cnum; 
LONG Icount; 
LONG hcount; 


switch (Port){ 
case CONSOLE: 
taddr = TIMERADDRESS_0; 
cnum = 1; 
break; 


case HOST: 
taddr = TIMERADDRESS_0; 
cnum = 2; 
break; 

case YSB1: 
taddr = TIMERADDRESS_1; 
cnum = 3; 
break; 

case YSB2: 
taddr = TIMERADDRESS_1; 
cnum = 4; 
break; 

defaut: 
PutStr(“i_Port: illegal port”); 
rexit(); 


} 


switch (Baud){ 

case 300: 
icount = 0x01a0; 
hcount = 0x01a1; 
break; 

case 1200: 
Icount = 0x0068; 
hcount = 0x0068; 
break; 

case 2400: 
lcount = 0x0034; 
hcount = 0x0034; 
break; 

case 4800: 
icount = 0x001a; 
hcount = 0x001a; 
break; 

case 9600: 
icount = 0x000d; 
hcount = 0x000d; 
break; 

case 19200: 
Icount = 0x0006; 
hcount = 0x0007; 
break; 

default: 
PutStr(“i_port: bad baud rate”); 
rexit(); 


} 
SetTimer(taddr, cnum, 0x0b62, icount, hcount); 


SetDataTransfer(Port, 
(HandShaking ? 0x20 : 0), 
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(TxInterrupt ? 0x02 : 0), 
(RxInterrupt ? 0x10 : 0)); 
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Function SetTimer() initializes one of the five counters located on the 

serial board (with the corresponding TimerAddress) by assigning values to 
command and data registers for that timer. Details on the values sent to the 
command and data registers can be found in the VME8300 Quad Serial Board 
Users Manual. 
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void SetTimer(LONG TimerAddress, 
LONG CounterNumber, 
LONG ModeCma, 
LONG LoadCmd, 
LONG HoldCma) 


BYTE* CmdRegister = (BYTE*)(TimerAddress + 2); 
BYTE* DataRegister = (BYTE*)(TimerAddress); 
BYTE CounterSelectBit; 

BYTE TimerCommand; 


CounterSelectBit = 1; 
CounterSelectBit = CounterSelectBit << (BYTE)(CounterNumber - 1); 


TimerCommand = DISARM; 
TimerCommand = TimerCommand | CounterSelectBit; 
*CmdRegister = TimerCommand; 


TimerCommand = LOAD; 
TimerCommand = TimerCommand | CounterSelectBit; 
*“CmdRegister = TimerCommand; 


*“CmdRegister = (BYTE)CounterNumber; 


*DataRegister = (BYTE)ModeCmd; 
*DataRegister = (BYTE)(ModeCmd >> 8); 
“DataRegister = (BYTE)LoadCmd; 
*DataRegister = (BYTE)(LoadCmd >> 8); 
*DataRegister = (BYTE)HoldCmd; 
“DataRegister = (BYTE)(HoldCmd >> 8); 


TimerCommand = ARM; 
TimerCommand = TimerCommand | CounterSelectBit; 
*CmdRegister = TimerCommand; 
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Function SetLatch() passes to the specified latch the index value of the 

vector table entry containing the interrupt handlers address to the serial 
board. When an interrupt associated with the given latch is generated, the 
VectorNumber is placed onto the address bus, indicating to the cpu the vector 
table entry that contains the address of the interrupt handler. 
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void SetLatch(LONG LatchAddress, BYTE VectorNumber){ 
*(BYTE*)LatchAddress = VectorNumber; 
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Function PutConsole() initiates the printing of the single character Source 
to the console. 
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void PutConsole(char Data){ 
PutB(Data, CONSOLE); 
} 
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Function PutConsole() initiates the printing of the single character Source 
to the host system. 
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void PutHost(char Data){ 
PutB(Data, HOST); 
} 
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Function GetConsole() polls the command/status register, waiting until a 
character from the console is placed in the receive buffer. It then returns 
that character to the calling function. 
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char GetConsole(){ 
BYTE* InputAddress = (BYTE*)CONSOLE; 
BYTE* InputCmd = (BYTE*)(CONSOLE + 2); 


“InputCmd = 0; 
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while((*InputCmd & 0x01) == 0) 


retum *(BYTE”*)InputAddress; 
} 
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Function TerminateConsoleOutput() signals the console port that the interrupt 
generated by the port has been serviced. 
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void TerminateConsoleOutput(){ 
*(BYTE*)(CONSOLE + 2) = 0x28; 


} 
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Function TerminateConsolelntCycle() tells the console port to terminate all 
unserviced interrupts. 
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void TerminateConsolelintCycle(){ 
*(BYTE*)(CONSOLE + 2) = 0x38; 


} 
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APPENDIX B 


A. IOSYS.H 
ae ea ean ee reir ee ke eRe ee eo ee nS ee anes 


Author(s): Scott Book 

Project: _Yamabico Robot Control System 

Date: December 8, 1993 

Revised: March 2, 1994 

File Name: iosys.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the prototypes/interface for the functions 
available in the I/O System module. 
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#itndet__IOSYS_H 
#define _|OSYS_H 


#include “definitions.h” 
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Function loSys!Initialize() initializes the ports used for serial communications. 
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void loSysInitialize(); 
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Function loSysControl() is the work-horse routine for the output interrupt 
handler. 
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void loSysControl(); 
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Function PutStr() initiates the printing of the characters in the Source 
string to the console. 
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void PutStr(char* Source); 
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Function Putint sends the ascii representation of the parameter Number to 
the console. 
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void Putint(int Number); 
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Function PutReal sends the ascii representation of the parameter Number to 

the console. The output is in exponential notation with a total of Places 

digits after the decimal (up to a maximum of the constant MAX_REAL_PRECISION 
found in the source file). 
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void PutReal(double Number, int Places); 
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Function GetStr() copies the string of characters typed at the console (up to 
Length characters) into the string pointed at by Source. 
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void GetStr(char* Source, int Length); 
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Function Getint() reads a string from the console (up to MAX_INTEGER_DIGITS) 
and converts the ascii representation into its integer value. All leading 

white space (spaces and tabs) are discarded. The conversion stops at the 

first character that can not be part of a legel integer value. All remaining 
characters in the string are discarded. Strings where the first non white- 

space character is not a digit will default to 0. Empty strings also defautt 

to 0. 
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int Getint(); 
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Function GetReal() reads a string from the console (up to MAX_REAL_PRECISION) 
and converts the ascii representation into its double value. All leading 
white space (spaces and tabs) are discarded. The conversion stops at the 
first character that can not be part of a legel double value. All remaining 
characters in the string are discarded. Strings where the first non white- 
space character is not a digit will default to 0.0. Empty strings also 

default to 0. 
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double GetReal(); 


#endif 


IOSYS.C 
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Author(s): Scott Book 
Project: _Yamabico Robot Control System 
Date: December 8, 1993 
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Revised: March 2, 1994 

File Name: iosys.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the routines and data structures needed to 
provide I/O capabilities to the rest of the subsystems in the 
Yamabico project. This file contains two marked sections. The 
first is the private section containing the encapsulated data 
and functions. The second section is the public section. This 
section defines the interface routines. 
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#include “definitions.h” 
#include “convertutil.h” 
#include “serial.h” 
#include “iosys.h" 
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PRIVATE SECTION 


The following section defines the encapsulated definitions and data 
structures used in the system. 
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#define BUFSIZE 1024 
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Structure and declaration of the output buffer. It is declared static to 
prevent access from routines external to this module. 
aia cee ee eg ene nee ae car meee ee cee me mene en ge meet eee 
typedef struct{ 
int Head; 
int Tail: 
int Count; 
char Buffer[BUFSIZE]; 
} IOBUFFER; 


static IOBUFFER OutputBuf; 
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PUBLIC SECTION 
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The following section defines the functions that provide access to the 
terminal system. 
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Function loSysInitialize() initializes the ports used for serial communications. 
The CONSOLE port is set for interrupt driven output, but direct memory (polled) 
input. The output buffer is also initialized. 
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void loSysinitialize(){ 
OutputBuf.Head = 0; 
OutputBuf. Tail = 0; 
OutputBuf.Count = 0; 
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Function loSysControl() controls the printing of characters in the output 
buffer to the screen. It is called from the interrupt handling routine. It 
operates by printing one character to the screen and then terminating the 
interrupt. The act of printing a character to the screen generates another 
interrupt to print the next character in the buffer. If the buffer is empty, 

it sends a command to the port to stop the interrupt chain. 
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void loSysControl(){ 


if(OutputBuf.Count > 0){ 
PutConsole(OutputBuf. Buffer OutputBuf. Tail]): 
OutputBuf.Count--; 
if(OutputBuf. Tail == BUFSIZE-1){ 

OutputBuf.Tail = 0; 
jelse 
OutputBuf. Tail++; 

Jelse 
TerminateConsoleOutput(); 

TerminateConsolelIntCycle(); 
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Function PutStr copies the string pointed to by Source into the output buffer. 
A critical region exists where the number of characters in the output buffer 
is incremented. Therefore, the priority mask is set to prevent any interrupts 
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from taking control of the CPU during its execution. 
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void PutStr(char* Source){ 
while (*Source |= ‘\0'){ 
OutputBuf.Count++; 


OutputBuf.Buffer{OutputBuf.Head] = “Source; 
Source++; 
if(OutputBuf. Head == BUFSIZE-1){ 
OutputBuf.Head = 0; 
Jelse 
OutputBuf.Head++; 
} 


/* Initiate an output interrupt by sending a null character to the console. */ 
PutConsole(‘\0’); 
} 
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Function Putlnt converts the parameter Number to its ascii representation and 
sends the resulting string to the console by calling to PutStr. 
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void Putint(int Number){ 
char NumStr20}; 


ItoA(Number,NumsStr); 
PutStr(NumStr); 


} 
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Function PutReal converts the exponential notation of the parameter Number 
into its ascii representation. It then sends the resulting string to the 
console by calling PutStr. 
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void PutReal(double Number, int Places){ 
char NumStrf{MAX_REAL_PRECISION+7]: 


if(Places<MAX_REAL_PRECISION){ 
RtoAE(Number,NumStr,Places+1); 

Jelse{ 
RtoAE(Number,NumStr, MAX_REAL_PRECISION+1); 


} 


US: 


PutStr(NumStr); 
} 
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Function GetStr() copies the string of characters typed at the console (up to 
Length characters) into the string pointed at by Source. 
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void GetStr(char” Source, int Length){ 
BYTE KeyStroke; 
int | = -1; 
int Size = Length - 1; 


do{ 
KeyStroke = GetConsole(); 


switch(KeyStroke){ 
case ‘\b’: 
if(l >= 0){ 
Fe; 
PutConsole(KeyStroke); 
} 
break; 
default: 
Source[++l] = KeyStroke; 
PutConsole(KeyStroke); 


} 
}while((Sourcefl] != \r') && (I < Size)); 


Source|l] = ‘\0’; 
} 
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Function Getint() reads a string from the console (up to MAX_INTEGER_DIGITS) 
and converts the ascii representation into its integer value. All leading 

white space (Spaces and tabs) are discarded. The conversion stops at the 

first character that can not be part of a legel integer value. All remaining 
characters in the string are discarded. Strings where the first non white- 

space character is not a digit will default to 0. Empty strings also default 

to 0. 
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int Get!nt(){ 
char NumStrfMAX_INTEGER_DIGITS+1]; 
char* Temp = NumStr; 
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GetStr(NumStr, MAX_INTEGER_DIGITS+1); 


| while((*Temp=="t’) || (“Temp==" ‘)) 
Temp++; 


retum Atol(Temp); 
} 
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Function GetReal() reads a string from the console (up to MAX_REAL_PRECISION) 
and converts the ascii representation into tts double value. All leading 

white space (spaces and tabs) are discarded. The conversion stops at the 

first character that can not be part of a legel double value. All remaining 

characters in the string are discarded. Strings where the first non white- 

space character is not a digit will default to 0.0. Empty strings also 

default to 0. 
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double GetReal(){ 
char NumStrfMAX_REAL_PRECISION+1]; 
char* Temp = NumStr; 
GetStr(NumStr, MAX_REAL_PRECISION+1); 


while((*Temp==‘t) || (“Temp==’ ‘)) 
Temp++; 


retum AtoR(Temp); 
} 
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APPENDIX C 


A. MOTION.H 
| kalashi ahd aaleaial ciate habe ged lakes al hb hl oho teh Belo hah heh hihi 


Author(s): Scott Book 

Project: _Yamabico Robot Control System 

Date: December 16, 1993 

Revised: March 2, 1994 

File Name: motion.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the prototypes/interface for the functions 
available in the Motion System module. The first two routines 
are for system setup and control. The rest are MML’s immediate 
commands defined by the language. 
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#ifndef MOTION_H 
#define _MOTION_H 


#include “definitions.h” 
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Function MotionSysinitialize() initializes the motion subsystem by assigning 
default values to the local variables and establishing the interrupt handling 
mechanism. 
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void MotionSysinitialize(); 


ia alll lalla dll halal cl aici Lah hl lal ole ESTERS II 


Function MotionSysControl() is the interrupt handler workhorse and is called 
from the assembly interrupt handler shell. 


didi lillie iad lll le a clea lala hades aha elilictal abalicli data dol A hhl Salto k AARON) 


void MotionSysControl(); 
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The following declarations are prototypes for MML’s immediate commands. 
The command descriptions can be found in the MML Bible. 


ata aalaataclataatalahlaaliah shall tial ahahaha cil sath 2 ha EA DDSI DRA RT 
void SetRobotConfiguration(CONFIGURATION NewConfiguration); 
void GetRobotConfiguration(CONFIGURATION* CurrentConfiguration); 


void Stop(); 
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void SetLinearVelocity(double LinearVelocity); 

void SetRotationalVelocity(double RotationalVelocity); 

void SetLinearAcceleration(double LinearAcceleration); 

void SetRotationalAcceleration(double RotationalAcceleration); 
void SetSizeConstant(double SizeConstant); 

double GetTotalDistance (); 

void SkipPathElement(); 

void HaltMotion(); 

void ResumeMotion(); 

CONFIGURATION SetinitialPosttion(); 

void ReportRobotConfiguration(CONFIGURATION CurrentConfiguration); 
void MotionOn(); 

void MotionOff(): 


#endif 


MOTION.C 
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Author(s): Scott Book 

Project: | Yamabico Robot Control System 

Date: December 16, 1993 

Revised: March 2, 1994 

File Name: motion.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file provides the routines and data structures needed to 
provide the motion control capability for the robot. The file 
is divided into three sections. The first is the private section 
containing the encapsulated data and functions. The second 
section is the control section. This section defines the 
routines required for motion control. The third section is the 
Immediate command section. This section defines MML’s immediate 
commands. The routines in these last two sections can be 
accessed publicly. 
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#include “definitions.h” 
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#include “system.h” 
#include “iosys.h” 
#include “wheels.h” 
#include “math.h” 
#include “math68881 .h” 
#include “motiontrace.h” 
#include “motion.h” 
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PRIVATE SECTION 


The following section defines the encapsulated definitions, data structures 
and prototypes used in the system. 
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#define SMALLERROR 0.0001 
Static int LoopTest; 
static BOOLEAN Halted; 


static VELOCITY HaltedVelocity; 
Static VELOCITY DesiredVelocity; 
static VELOCITY Commanded; 

static VELOCITY DesiredAcceleration; 


static double TotalDistance; 
static double DesiredSizeConstant;: 


static CONFIGURATION VehicleConfiguration; 


| haaahalaallalaialialaialaalaih cian aa lal il aaah lal Nah al NSF Stasi 


The following static function declarations are the prototypes for the 
encapsulated functions. 
hhlaieiallenhaheieletaileliialiaialalelaii ania shai alah hh aight PALI AL be hed 
static void UpdateConfiguration(double DeltaDistanceChanged, 
double DettaOrientation); 
Static VELOCITY GetCommandedVelocity(VELOCITY Desired, 
VELOCITY Actual, 
VELOCITY Commanded); 
Static double GetLinearVelocity(double rei ah iain 
double ActualVelocity, 
double LastCommandedVelocity); 
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Function UpdateConfiguration() calculates the robots current position based 
on DeltaDistanceChanged (ds) and DeltaOrientation (dt). 
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Static void UpdateConfiguration(double DeltaDistanceChanged, 
double DeltaOrientation){ 


double Distancelncrement = DeltaDistance Changed; 
double OrientationIncrement = DeltaOrientation / 2; 


Disablelnterrupts(); 


it(fabs(DeltaOrientation) > SMALLERROR) 
Distancelncrement *= sin(OrientationIncrement) / Orientationincrement; 


VehicleConfiguration. Position. XPosition += Distancelncrement * 
cos(VehicleConfiguration.Orientation + Orientationincrement); 
VehicleConfiguration.Position. YPosition += Distancelncrement * 
sin(VehicleConfiguration.Orientation + OrientationIncrement); 
VehicleConfiguration. Orientation += DeltaOrientation; 
it(fabs(DeltaDistanceChanged) > SMALLERROR){ 
VehicleConfiguration.Kappa = DeltaOrientation / DeltaDistanceChanged; 
Jelse{ 
VehicleConfiguration.Kappa = DeltaOrientation / SMALLERROR; 


} 


EnableInterrupts(); 
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Function GetCommandedVelocity() calculates the commanded velocity based on 
the current velocity, the desired velocity, and the previous commanded 
velocity. 
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static VELOCITY GetCommandedVelocity(VELOCITY Desired, 
VELOCITY Actual, 
VELOCITY Commanded){ 
Commanded.Linear = 
GetLinearVelocity(Desired.Linear, Actual.Linear,Commanded.Linear); 


/* This statement is used since GetRotationalVelocity() is not */ 


/* currently defined. Otherwise, a statement similar to above */ 
/* would be used. : 


no 


Commanded.Rotational = Desired.Rotational, 


retum Commanded; 


} 
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Function GetLinearVelocity() calculates the linear component of the commanded 
velocity. 
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static double GetLinearVelocity(double DesiredVelocity, 
double ActualVelocity, 
double CommandedVelocity){ 
double VelocityChange; 


VelocityChange = DesiredAcceleration.Linear * MOTION_CONTROL_CYCLE; 


if(ActualVelocity < DesiredVelocity){ 

CommandedVelocity = Min(CommandedVelocity + VelocityChange, DesiredVelocity); 
jelse{ 

CommandedVelocity = Max(CommandedVelocity - VelocityChange, DesiredVelocity); 


} 


retum CommandedVelocity; 
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MOTION CONTROL SECTION 


The following section defines the functions that provide access to the 
motion control system. These routines are public. 
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Function MotionSys!nitialize() initializes all of the private global variables 

in this module to the desired default values. It then calls SetTimer to 

program the 5th timer on senial board #1 (the second serial board) to generate 
synchronous interrupts every 10ms. After the timer has been set up, the 
interrupt handling routine is made available to the system by the call to 
SetMotionInterruptHandler(). 
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void MotionSyslinitialize(){ 
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LoopTest = 0; 
Halted = FALSE; 
TotalDistance = 0.0; 


DesiredVelocity.Linear = 0.0; 
DesiredVelocity.Rotational = 0.0; 
Commanded.Linear = 0.0; 
Commanded.Rotational = 0.0; 
DesiredAcceleration.Linear = 20.0; 
DesiredAcceleration.Rotational = 0.5; 
DesiredSizeConstant = 20.0; 


VehicleConfiguration.Position.XPosition = 0.0; 
VehicleConfiguration. Position. Position = 0.0; 
VehicleConfiguration.Orientation = 0.0; 
VehicleConfiguration.Kappa = 0.0; 


/* Initialize sub-systems. */ 
InitializeWheels(); 
TraceMotionSyslnitialize(400); /“ This sub-system only required ff data */ 
/* logging is desired. tL 


SetMotionintMechanism(); 


} 
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Function MotionSysControl() is the interrupt handler workhorse. It is called 
from the assembly interrupt handler shell. Its first task is to update the 
change in position and orientation through calls to the module responsible for 
movement. It then uses this information in the motion control laws to derive 
the commanded linear and rotational velocities required for this motion 
control cycle. Finally, tt passes these computed velocities back to the move- 
ment module for execution. 
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void MotionSysControl(){ 
double OrientationChange; 
double DistanceChanged; 
VELOCITY Actual; 


UpdateMovement(); 
DistanceChanged = GetDistance Traveled(); 
OrientationChange = GetOrientationChange(); 


TotalDistance += DistanceChanged; 
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UpdateConfiguration(DistanceChanged, OrientationChange); 


Actual.Linear = DistanceChanged / MOTION_CONTROL_CYCLE; 
Actual.Rotational = OrientationChange / MOTION_CONTROL_CYCLE; 


/* The logging statement can be moved, modified or deleted as desired. */ 
LogTimedMotion(Actual.Linear); 


Commanded = GetCommandedVelocity(DesiredVelocity Actual,Commanded); 
SetMovement(Commanded.Linear,Commanded.Rotational); 


/* LoopTest used to control output from interrupt driven motion control */ 
/* system. LoopTest is assigned zero every 100 cycles (1 sec). */ 
if(LoopTest++ >= 99) 

LoopTest = 0; 
} 


eee eee ee ee eR eee eee nee ee | eae cece" ~ camilla alee 


IMMEDIATE COMMAND SECTION 


The following section defines the functions that provide access to MML’s 
immediate commands. The functionality of these routines can be found in the 
language definition. These routines are also public. 
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void SetRobotConfiguration(CONFIGURATION NewConfiguration){ 
Disablelnterrupts(); 


VehicleConfiguration.Position.XPosition = NewConfiguration.Position.XPosition; 
VehicleConfiguration.Position.YPosition = NewConfiguration.Position. YPosition; 
VehicleConfiguration.Orientation = NewConfiguration.Orientation; 
VehicleConfiguration.Kappa = NewConfiguration.Kappa; 


Enablelnterrupts(); 


void GetRobotConfiguration(CONFIGURATION®’ CurrentConfiguration){ 
Disablelnterrupts(); 
*“CurrentConfiguration = VehicleConfiguration; 
Enablelnterrupts(); 


} 
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void Stop(){ 
Wheels Disable(); 
DesiredVelocity.Linear = 0.0; 
DesiredVelocity. Rotational = 0.0; 


} 


void SetLinearVelocity(double LinearVelocity){ 
DesiredVelocity.Linear = LinearVelocity; 


} 


void SetRotationalVelocity(double RotationalVelocity){ 
DesiredVelocity.Rotational = RotationalVelocity; 


} 


void SetLinearAcceleration(double LinearAcceleration){ 
DesiredAcceleration.Linear = LinearAcceleration; 


} 


void SetRotationalAcceleration(double RotationalAcceleration){ 
DesiredAcceleration.Rotational = RotationalAcceleration; 


void SetSizeConstant(double SizeConstant){ 
DesiredSizeConstant = SizeConstant; 


} 


double GetTotalDistance (){ 
retum TotalDistance; 


} 
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void SkipPathElement(){ 
PutStr(‘Aninside skip() stub”); 


} 


void HaltMotion(){ 
if(lHalted){ 
Halted = TRUE; 
HaltedVelocity.Linear = DesiredVelocity.Linear; 
HaltedVelocity.Rotational = DesiredVelocity.Rotational; 
WheelsDisable(); 


} 
} 


void ResumeMotion(){ 
if(Halted){ 
Halted = FALSE; 
DesiredVelocity.Linear = HaltedVelocity.Linear; 
DesiredVelocity.Rotational = HalttedVelocity.Rotational:; 
WheelsEnable(); 
} 
} 


CONFIGURATION SetinitialPosition(){ 
CONFIGURATION NewConfiguration; 


PutStr(“nEnter the Starting X Position: “); 
NewConfiguration.Position.XPosition = GetReal(); 
PutStr(“Enter the Starting Y Position: “); 
NewConfiguration.Position.YPosition = GetReal(); 
PutStr(“Enter the Starting Orientation: “); 
NewConfiguration.Orientation = GetReal(); 
PutStr(“Enter the Starting Kappa Value: “); 
NewConfiguration.Kappa = GetReal(); 


retum NewConfiguration; 


void ReportRobotConfiguration(CONFIGURATION CurrentConfiguration){ 
PutStr(“AnCurrent Robot Configuration:\n\tX =>\t\t"); 
PutReal(CurrentConfiguration.Position.XPosition,4); 
PutStr(‘An\tY =>\t\t’); 
PutReal(CurrentConfiguration.Position. Y Position, 4); 
PutStr(‘An\tTheta =>\t"); 
PutReal(CurrentConfiguration.Orientation,4); 
PutStr(‘An\tKappa =>\t"); 
PutReal(CurrentConfiguration.Kappa,4); 


void MotionOn(){ 
WheelsEnable(); 


void MotionOff(){ 
WheelsDisable(); 


} 


MOTIONTRACE.H 
FN I a a a al lic lal lala als 


Author(s): Scott Book 

Project: Yamabico Robot Control System 

Date: January 20, 1994 

Revised: March 3, 1994 

File Name: motiontrace.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the prototypes/interface for the functions 
available to log motion control data. 


ag ye eee ed ane Steere ene eT een eee eee Tee re ee ey 


#ifndef MOTIONTRACE_H 
#define MOTIONTRACE_H 


[PSOE DEE Dalal Sk al i laa ala ala lial 


Function TraceMotionSysinitialize() prepares the tracing system to log data. 

It requests a block of dynamic memory to store the number of data points 
requested by NumberOfPoints. No error checking for dynamic memory allocation 
is performed by this function. 


85 


lalalaliii all clial ialeal alin! ial lalla eck cl ahh Eh hata Sahl h NEA BRAS I SRA NANI 


void TraceMotionSysinitialize(int NumberOfPoints); 


ahahaha halal alia llialahalaliahialahaialalabein ahi ch a ghia li ebcf cto hI NINN 


Function MotionTraceEnable() enables data logging. Frequency is used by the 
system's logging functions to determine the number of motion control cycles 
between logged data. For example, if the frequency is 3, then data would be 
logged on every third call to LogTimedMotion() or LogMotionData(). 


lated alah alah lle alah tial leh lal liebe) oh Kael Soll tdaae hd Anhighed AAT 


void MotionTraceEnable(int Frequency); 
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Function MotionTraceDisable() disables data logging. 
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void MotionTraceDisable(); 
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Function LogTimedMotion() logs Data against time in seconds, starting when 
data logging is turned on. This is based the assumption that the routine is 
called every motion control cycle. The Frequency value given when logging is 
enabled determines the number of number of LogTimedMotion() calls required 
between recorded data. 


la ce aac chat lla helialalalaalalabonghchal catia cliche halk ilk altel ht loh tl AAA) 


void LogTimedMotion(double Data); 


haliimlaaltiasa tk liked athe lalallala al iol lO ah ahead ich i oho 


Function LogMotionData() logs both parameters after every ‘Frequency’ calls. 
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void LogMotionData(double XPlot, double YPlot); 


| plaka alahalitiah cll lel il cial cl clinl hh FD NA Sl Ba fall la Salas Lah hh SE 


Function DownLoadMotionData() prompts the user for a file name on the host 
system to store the data. The file then opened, deleting any previous 
contents. The routine then transfers character string representations of the 
recorded data to the Unix host. 


Be eee eR ee ee ee eo renee maa gir ee giet yc miele tS ocr Ae alee eed eee 


void DownLoadMotionData(); 


#endif 
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D. MOTIONTRACE.C 
igor Soc Geena es eee ets Ss ee ee ee ee rn ee 


Author(s): Scott Book 

Project: | Yamabico Robot Control System 

Date: January 20, 1994 

Revised: March 3, 1994 

File Name: motiontrace.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the routines required to for the tracing 
system. The file is seperated into two section. The first is 
the private section containing the encapsulated data and 
functions. The second section is the public section. This 
section defines the interface routines. 


NN a a al aa a aa ala allie | 


#include “definitions.h” 
#include “memsys.h” 
#include “convertutil.h” 
#include “senial.h” 
#include “iosys.h” 
#include “motiontrace.h” 


PR ire eee rye ee rege see eee en ere te er ana, eee er eee 


PRIVATE SECTION 


The following section defines the encapsulated definitions, data structures 
and prototypes used in the system. 


RI rag ee re erg eee ee enor eo eee ake a 


typedef struct{ 
double XData; 
double Y Data; 

} SAMPLINGPOINT; 


Static SAMPLINGPOINT *MotionData; 


Static int MaxDataPoints; 
Static int NextPlot; 

Static int LoggingData; 
Static int CycleCounter; 
Static int LoggingFrequency; 


FORTIES EE tA El al al lll lhl thal lla kod lal alas halalal sidiclalal 


The following static function declarations are the prototypes for the 
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encapsulated functions. 


al aha ah ahaa ll a aca LIES lel had Aah lal lag ol lh talib A fies IY, 


static void PutStrHost(char* Source); 


allah hahahahahahaha lk deh li ato hh Sail sll EID DAIS 


Function PutStrHost() is a simple routine that writes a character string to 

the host environment. Writing non-character information to the host 
environment may produce unexpected results. Other routines may be created to 
write non-character data by converting the data to character strings prior 

to being sent. 


ahahaha ahalalaah hla llallalalalihaalia i lalalaiadedeis PPL EIS DIDI ALR NA 


Static void PutStrHost(char* Source){ 
while (“Source f= ‘\0’){ 


PutHost(*Source); 
Source++; 
} 
} 


| halal dialed hlalalahah alesis Rabi tel! SA KR ga ah aS a al lla laches ctl he alba tae Sal! inh slot Sh! 


PUBLIC SECTION 


The following section defines the functions that provide access to the 
tracing sub-system. 


Me eee eee MUN marsteipcmier cht Sn mien cea ga eae gee ct esi hoe oe i la aa 


ete eee ee eee ee ee we me Sie mem ce wie graeme ela a ae 


Function TraceMotionSysinitialize() prepares the tracing system to log data. 

It requests a block of dynamic memory to store the number of data points 
requested by NumberOfPoints. No error checking for dynamic memory allocation 
is performed by this function. 


adalah ak lll alll laliclalalalialalala 8 TAL 


void TraceMotionSysinitialize(int NumberOfPoints){ 
NextPlot = 0; 
LoggingData = FALSE; 
CycleCounter = 0; 
LoggingFrequency = 1; 


it(NumberOfPoints > 0) 
MaxDataPoints = NumberOfPoints; 
else 


MaxDataPoints = 0; 
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free((BYTE*)MotionData); 
MotionData = (SAMPLINGPOINT“*)malloc(sizeof(SAMPLINGPOINT)*NumberOfPoints); 


} 


TOE a a a ail aa ie la aaa 


Function MotionTraceEnable() enables data logging. Frequency is used by the 
system’s logging functions to determine the number of motion control cycles 
between logged data. For example, if the frequency is 3, then data would be 
logged on every third call to LogTimedMotion() or LogMotionData(). 


i eee Se pee ee ne ee eee ee eee ee ewe ee a ey 


void MotionTraceEnable(int Frequency){ 
it(Frequency > 0) 
LoggingFrequency = Frequency; 
else 
LoggingFrequency = 1; 


LoggingData = TRUE; 


(OI eae i ta aaa al ln alg at 


Function MotionTrace Disable() disables data logging. 


Mueieianehtes cg gre camer a eg eee ne © Reem eg ee ere 


void MotionTrace Disable(){ 
LoggingData = FALSE; 


egy ee ces eyes ee ie ee cee Se ete Ten eee eT ee eee rr eo a 


Function LogTimedMotion() logs Data against time in seconds, starting when 
data logging is turned on. This is based the assumption that the routine is 
called every motion control cycle. The routine uses the variable CycleCounter 
as a counter. When the value of CycleCounter reaches zero, data is logged and 
the counter is reset to the LoggingFrequency set when logging was enabled. If 
the value is greater than zero, the counter is decremented and logging is NOT 
performed. 


ns seco ogee ee ee eee ns re ee es 


void LogTimedMotion(double Data){ 
if(LoggingData){ 
if((CycleCounter-- <= 0) && (NextPlot < MaxDataPoints)){ 
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MotionData[NextPlot].xData = NextPlot “ LoggingFrequency 
*“ MOTION_CONTROL_CYCLE; 

MotionData[NextPlot].YData = Data; 

NextPlot++; 

CycleCounter = LoggingFrequency; 


| hahahaha ali lah lala aad lial lal aledalc ic aah! 8 of hoch ER DBPL DNL DR A Re READ NS 


Fucntion LogMotionData() records both parameters when the CycleCounter is 
zero. It then resets the counter to LoggingFrequency, assigned when logging 
is enabled. If the value is greater than zero, the counter is decremented and 
logging is NOT performed. 


halal lhl alata alae hail cial il ll al lalad hal gE EIEN 


void LogMotionData(double XPlot, double YPlot){ 
if(LoggingData){ 
if((CycleCounter-- <= 0) && (NextPlot < MaxDataPoints)){ 
MotionData[NextPlot].xData = XPlot; 
MotionData[NextPlot].YData = YPlot; 
Next Plot++; 
CycleCounter = LoggingFrequency; 
} 
} 
} 


eee ee we Pn pr aeers erga ei sein Ort an esa lca fee cece ed 


Function DownLoadMotionData() prompts the user for a file name on the host 
system to store the data. The file then opened, deleting any previous 
contents. The routine is dependent on the “ytof” call in the yamabico 

account. The routine then transfers character string representations of the 
recorded data to the Unix host. Afterwards, the function ensures that the 

host file is closed. 


aaah ial acai il ai aa EE II RSLS hall hE 


void DownLoadMotionData(){ 
char FileName[35]; 
char DataString[jMAX_REAL_PRECISION]; 
int LoopCounter = MaxDataPoints; 


MotionTraceDisable(); 
PutStr(“An\n\aReady to down load motion data. Connect the”); 


PutStr(‘Ancable and press any key to continue.”); 
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GetConsole(); 


PutStr(‘\a\n\nEnter the name of the Host file used to store the Motion Data”); 
PutStr(“n(WARNING: PREVIOUS FILE CONTENTS WILL BE DESTROYED)"); 
PutStr(‘An\n\tFile: “); 

GetStr(FileName,35); 


PutStrHost(“ytof “); 
PutStrHost(FileName); 
PutStrHost(“ w \n’); 


PutStr(‘An\n\aReady to down load data to Host computer. “); 
PutStr(‘\nPress any Key to begin “); 
GetConsole(); 


PutStr(‘An\nDown loading data...”); 


for(LoopCounter=0; LoopCounter<MaxDataPoints; LoopCounter++){ 
PutStrHost(RtoAE(MotionData[LoopCounter].XData,DataString,5)); 
PutStrHost(‘\t"); 
PutStrHost(RtoAE(MotionData[LoopCounter]. Y Data, DataString,5)); 
PutStrHost(“\n’); 


} 


PutHost(\4’); 
PutHost(‘14’); 


PutStr(aDown load complete”); 


E. WHEELS.H 
Pe ro ge ee ree eee ee errr e eee re ene eens 


Author(s): Scott Book 

Project: Yamabico Robot Control System 

Date: December 16, 1993 

Revised: March 2, 1994 

File Name: wheels.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains the prototypes to available wheel sub-system 
routines. This set of prototypes defines the wheel sub-system 
interface. 


ea annee Gaede? ed aN ee oe pe ee ep 


#ifndef — WHEELS _H 
#define — WHEELS H 


#include “definitions.h” 
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#define MAXVELOCITY 60 


| halal i ah adalah hahah il la Th halla! fh INS AAR Dh Nh A 


Function InitializeWheels() initializes the wheels subsystem to its default 
Settings. 


i halal altel ed ill alana eld ni tl oh Soh A Nh DAL heh BA SNA ALAS EA BR AL | 


void InitializeWheels(); 
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Function WheelsEnable() turns on the motors connected to the wheels. 
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void WheelsEnable(); 
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Function WheelsDisable() turns off the motors connected to the wheels. 


halhaladiaadahaahachataalala halal hadi al lial! iad lag PP RDI ARB LDA Red SD alc hella 


void Wheels Disable(); 


halal lh ia a nL i NA a aia aad adie i 


Function UpdateMovement() updates the distance traveled by both wheels. 


aa akan hla lich hich A SA? 


void UpdateMovement(); 


| halal delice cacti hl a hh NIRS 


Function GetDistanceTraveled() retums the linear distance the robot has 
traveled between the last two calls to Update Movement(). 


ala ld aaa hcl hal ic gh ic ialalatgh eke! on Nh SAN AA 


double GetDistanceTraveled(); 


rere eee ee eee eee cena eee ee ee timing cr 7 -eameiaieieec 5 Sica aie eel ii 


Function GetOrientationChange() returns the difference between the changes 
in the distance of the left and right wheels between the last two calls to 
UpdateMovement(). 
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double GetOrientationChange(); 
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Function SetMovement() translates the commanded linear and rotational vel- 
ocities into commanded velocities for each wheel. 


alll ath ha aah all hc aad hilo AchoclicE 


void SetMovement(double LinearVelocity, double RotationalVelocity); 


#endif 
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F. WHEELS.C 
| eee er ae ey eae ees eee e ene ere ee een een 


Author(s): Scott Book 

Project: _Yamabico Robot Control System 

Date: December 17, 1993 

Revised: March 2, 1994 

File Name: wheels.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file defines the wheel sub-system. It contains the hardware 
interface routines required for operation with the Dual Axis 
Controller. The file is seperated into two section. The first 
is the private section containing the encapsulated data and 
functions. The second section is the public section. This 
section defines the interface routines. 


cet eee ea ce soe eee ee ee ne ene eee MOL Seren nee Gree f 


#include “definitions.h” 
#include “wheels.h” 


ee oe ee ge eee ee SNe Ses 


PRIVATE SECTION 


The following section defines the encapsulated definitions, data structures 
and prototypes used in the system. 


nema nee ree en ge eee ere ea ee ee) 


#define RIGHT_DRIVE_PWM Oxtffff000 
#detine RIGHT BRAKE PWM Oxtffff002 
#detine LEFT_DRIVE_PWM Oxtffff004 
#define LEFT_BRAKE_PWM Oxtffff006 


#define MOTIONCONTROLADDRESS Oxfffff008 
#define RIGHT _ENCODER_HIGH_WORD Oxfffff010 
#define RIGHT_ENCODER_LOW_WORD Oxfffff012 
#define LEFT_ENCODER_HIGH_WORD Oxfffff014 
#define LEFT ENCODER _LOW_WORD Oxfffff016 


#define TREAD 52.41246478129945756620 
#define TREAD_R 53.40092 /* width of the robot (cm) when rotating. */ 


#define ENC_TO_DIST 0.00105570833333333333 


5 


#define KPWB 10.0 


/* Assumed PWM range: 0..127. Set at 90 to allow testing on weak AC generator. */ 
/* Can be changed to 127 if generator not used or upgraded. ai 
#define PWMLIMIT 90 


static int MotorOn; 
static BOOLEAN Rotating; 


Static int RightEncoderValue; 
Static int LeftEncoderValue; 

static int DeltaRightEncoderValue; 
Static int DeltaLeftEncoderValue; 


static double Rpowm; 
Static double Lpwm; 
static WORD MotionControlWord; 


italia tial aad callaalielag li lis ee el hed lel hati ai sched dil hs ae IN 


The following static function declarations are the prototypes for the 

encapsulated functions. 
Ree eee eee eee eee eee came ens ieee cee cea 
Static int GetWheelEncoder(LONG HighWordAddress, LONG LowWordAddress); 
Static int EncoderDifference(int NewValue, int OldValue); 
Static int GetPwm(double CommandedVelocity, double ActualVelocity, double Pwm); 
Static double PwmLookUp(double Velocity); 
Static void SetMotorControl(int Lpwm, int Rpwm, int MotionControlWord); 


Penne ee irene wry ey cogs ert ot Wo me Or cg ee ete re 


Function GetWheelEncoder() appends the contents of the shaft encoder low 
word register to the contents of the high word register, forming a long 
word. it then returns that value. 


ee ee eee eee eee eee ee ee ee ee eee eee ene Ss © oe meelel ater caramel a aed alee een | 


Static int GetWheelEncoder(LONG HighWordAddress, LONG LowWordAddress){ 
LONG Wheel; 


Wheel = *(WORD"*)HighWordAddress; 
Wheel = Wheel << 16; 
Wheel += *(WORD*)LowWordAddress; 


retum Wheel: 
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Function EncoderDifference() returns the difference between the new shaft 
encoder position and the old shaft encoder position. The shaft encoder values 
contain only 24 bits (0x000000-Oxffffff). The routine adjusts for the trans- 

ition from Oxffffff to Ox000000 and vice versa. 


eee ce er gr et rn rede ig ge ne On eer nar ee a se ee es pine ee ae 


Static int EncoderDifference(int NewValue, int OldValue){ 
int Difference = NewValue - OldValue; 


if(Difference < -0x800000){ 
Difference = Difference & OxOOfftfff; 
Jelse if(Difference >= 0x800000){ 
Difference = Difference | OxffO00000; 


} 


retum Difference; 


} 


ieee tease oer ee oe ag ey ee ee nye nee Gee epee eR cee ce eee ee 


Function GetPwm() returns a new PWM value based on the desired velocity, 
the actual velocity and the old PWM value. 


eS Foy ee en ee ge eee rr Ome re ee eg ere ee ee 


Static int GetPwm (double CommandedVelocity, double ActualVelocity, double Pwm){ 
double a = 0.7; 
int PwmTemp = PwmLookUp(CommandedVelocity) + KPWB * 
(CommandedVelocity - ActualVelocity); 


retum (a “ PwmTemp + (1.0 - a) “ Pwm); 
} 


ie doer eee eee ea eee ne eee ge ree ret ee eee ee any eee 


Function SetMotorControl() sends the PWM values to the motor control board 
if the motors are turned on via a previous call to MotorEnable(). If the 
motors are not turned on, then a command to free the motors is sent. 


MRA py etre es ee ner nar Oe nage anne ee coe ete ae aha ae ce ee elo ef 


Static void SetMotorControl(int Lowm, int Rowm, int MotionControlWord){ 
if(MotorOn){ 
*(WORD*)LEFT_DRIVE_PWM = (WORD)Lpwm; 
*(WORD*)RIGHT_DRIVE_PWM = (WORD)Rpwm; 
*(WORD*)MOTIONCONTROLADDRESS = (WORD)MotionContro!lWord; 
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Jelse{ 
*(WORD*)MOTIONCONTROLADDRESS = 0x0303; 
} 
} 


[ed ce ee ee ene en re ele nt aa 


FUNCTION: pwm_lookup 
PARAMETERS: vel (wheel velocity) 
PURPOSE: Determines the estimated pwm ratio given 
the desired wheel velocity as an input. (This table get from 7.9 KHZ 
motor output curve). 
RETURNS: pwm value based upon empirically determined velocity 
vs pwm ratio curve. 
CALLED BY: control() 
CALLS: none 
COMMENTS: 12 Jan 93 - Dave MacPherson,16 Sep 1993 changed by Ten-Min Lee 
TASK: Level 4 interrupt 


Bee eee eee eee ee ere rem te mnt ey cr net gle ce 


Static double PwmLookUp(double Velocity){ 
double v; 
double pwm_value; 


v = Velocity; 

if (v==0.0) 
pwm_value = 0.0; 

else if (Vv >= 0.0 && v < 25.0) 
pwm_value = (0.96 * v + 49.0); 

else if (V>= 25.0 && v < 53.0) 
pwm_value = (0.82 * (v - 25.0) + 73.0); 

else if (V >= 53.0 && v <= 65.0) 
pwm_value = (2.0 * (v - 53.0) + 96.0); 

else if (v > 65.0) 
pwm_value = 127.0; 

else if (V< 0.0 && v >= - 2.5) 
pwm_value = (1.2 * (v ) - 54.0); 

else if (Vv < -2.5 && v >= -13.0) 
pwm_value = (0.76 * (v + 2.5) - 57.0); 

else if (V < -13.0 && v >= -20.0) 
pwm_value = (0.43 * (v + 13.0) - 65.0); 

else if (v < -20.0 && v >= -34.0) 
pwm_value = (1.0 * (v + 20.0) - 68.0); 

else if (v < -34.0 && v >= -41.0) 
pwm_value = (0.7 * (v + 34.0) - 82.0); 

else if (Vv < -41.0 && v >= -49.0) 
pwm_value = (1.5 * (v + 41.0) - 87.0); 

else if (v < -49.0 && v >= -62.0) 
pwm_value = (1.1 * (v + 49.0) - 99.0); 
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else if (v < -62.0 && v >= -65.0) 
pwm_value = (2.3 * (v + 62.0) - 113.0); 
else 
pwm_value = -127.0; 


return pwm_value; 
} /* end pwm_lookup */ 
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PUBLIC SECTION 


The following section defines the functions that provide access to the 
wheel sub-system. 


Rar rn Se ei ee eee ee nn ee oie meee ne ere mee me ey 
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Function InitializeWheels() initializes all of the private global variables 
in this module to the desired default values. 


ey < hdc gon execu oe age ee ee es ep 


void InitializeWheels(){ 
Wheels Disable(); 
Rotating = FALSE; 


Rpwm = 0.0; 
Lowm = 0.0; 
MotionControlWord = 0: 


DeltaRightEncoderValue = 0; 
DeltaLeftEncoderValue = 0; 
RightEncoderValue 
GetWheelEncoder(RIGHT_ENCODER_HIGH_WORD,RIGHT_ENCODER_LOW_WORD); 
LeftEncoderValue 
GetWheelEncoder(LEFT_ENCODER_HIGH_WORD,LEFT_ENCODER_LOW_WORD); 
} 
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Function WheelsEnable() turns on the motors connected to the wheels. 
This is done by setting the MotorOn flag, used by SetMotorControl(), to 
a non-zero value. 


ne pe ee ee ee ene ee ne te eee See eee nee ey 


void WheelsEnable(){ 


| 


MotorOn = 1; 
} 


hala alia alia aaa elite hk cad lac a Solel BSL hale dal nkhalid Sorel NP 


Function WheelsDisable() turns off the motors connected to the wheels. 
This is done by setting the MotorOn flag, used by SetMotorControl(), to 
zero. 
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void WheelsDisable(){ 
MotorOn = 0; 


} 
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Function UpdateMovement() updates the distance traveled by both wheels. It 
does this by calculating the difference between the current shaft encoder 
values and the encoder values from the last time they were read. It also 
Stores the current encoder values which will be used as the last encoder 
values when this routine is called again. 


eRe ee ae ee ge ee ee eee ee ene eg aoe ale tpt a agama goes a eile ee 


void UpdateMovement(){ 
int Wheel; 


Wheel 
GetWheelEncoder(RIGHT_ENCODER_HIGH_WORD,RIGHT_ENCODER_LOW_WORD); 
DeltaRightEncoderValue = EncoderDifference(Wheel, RightEncoderValue); 
RightEncoderValue = Wheel; 


/* The left motor is the mirror image of the right. Therefore the */ 
/* amount of change in the left encoder value needs to be negated */ 
/* to show the proper direction of rotation. */ 

Wheel 
GetWheelEncoder(LEFT_ENCODER_HIGH_WORD,LEFT_ENCODER_LOW_WORD); 
DeltaLeftEncoderValue = - EncoderDifference(Wheel,LeftEncoderValue); 

LeftEncoderValue = Wheel: 


} 


| finland id ohh ahah hail ala lala hi load a EE II ICA IIIA 


Function GetDistanceTraveled() retums the linear distance the robot has 
traveled between the last two calls to UpdateMovement(). It makes the 
calculations based on the measured center point between the two wheels by 
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taking the average change between them. 
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double GetDistanceTraveled(){ 
double DistanceRight; 
double DistanceLeftt; 


DistanceRight = (double)DeltaRightEncoderValue * ENC_TO_DIST; 
DistanceLeft = (double)DeltaLeftEncoderValue * ENC_TO_DIST; 


retum ((DistanceRight + DistanceLeft) / 2.0); 


ei g  ae FO eng ie mr he ee ee eee ene ae ete 


Function GetOrientationChange() returns the difference between the changes 

in the distance of the left and right wheels between the last two calls to 
UpdateMovement(). It makes the calculations based on the center point between 
the two wheels. MML10 made a distinction based on whether the robot was 
rotating or not when calculating the center point between the wheels. This 
distinction is included in this routine. 
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double GetOrientationChange(){ 
double DistanceRight; 
double DistanceLeft; 
double OrientationChange; 


DistanceRight = (double)DettaRightEncoderValue * ENC_TO_DIST; 
DistanceLeft = (double)DettaLeftEncoderValue * ENC_TO_DIST; 


if(Rotating){ 

OrientationChange = (DistanceRight - DistanceLeft) / TREAD_R; 
jelse{ 

OrientationChange = (DistanceRight - DistanceLeft) / TREAD; 


} 


retum OrientationChange; 


CR ene eee ee ee ne re ee ee ce ane roo ee een sear enero Tea. 


Function SetMovement() translates the commanded linear and rotational vel- 
ocities into commanded velocities for each wheel. It then calculates the PWM 
values for each wheel and calls SetMotorControl() to execute PWM commands. 


eRe ca nen Cun eve enero gee ee ee eee ge we ee ee eee ge eee ee 


i 


void SetMovement(double LinearVelocity, double RotationalVelocity){ 
double RightWheelVelocity; 

double LeftWheelVelocity; 

double CommandRightVelocity; 

double CommandLeftVelocity; 

double Tread2; 

int RowmTemp; 

int LowmTemp; 


RightWheelVelocity DeltaRightEncoderValue 


MOTION_CONTROL_CYCLE; 


DeltaLeftEncoderValue - 


LeftWheelVelocity 


MOTION_CONTROL_CYCLE; 


if(Rotating){ 

Tread2 = 0.5 * TREAD_R; 
jelse{ 

Tread2 = 0.5 * TREAD; 


CommandLeftVelocity = LinearVelocity - (Tread2 * RotationalVelocity); 


ENC_TO_DIST 


ENC_TO_DIST 


CommandRightVelocity = LinearVelocity + (Tread2 * RotationalVelocity); 


LpwmTemp = GetPwm(CommandLeftVelocity, LeftWheelVelocity, Lowm); 
RpwmTemp = GetPwm(CommandRightVelocity, RightWheelVelocity, Rpwm); 


Lpwm = LpwmTemp; 
Rpowm = RpwmTemp; 


MotionControlWord = (MotionControlWord & OxfOf0) | 
((LowmTemp > 0) ? 1: 2) | 
((RpowmTemp > 0) ? 0x100 : 0x200); 


it(RpwmTemp > PWMLIMIT){ 
RpwmTemp = PWMLIMIT; 

Jelse if(RpwmTemp < -PWMLIMIT){ 
RpwmTemp = -PWMLIMIT; 


} 


if(LowmTemp > PWMLIMIT){ 
LowmTemp = PWMLIMIT; 

Jelse if(LpwmTemp < -PWMLIMIT){ 
LowmTemp = -PWMLIMIT; 


} 


SetMotorControl(LpwmTemp, RowmTemp, MotionControlWord); 
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APPENDIX D 


A. USER.H 
ee a ae aye eee er in Gee eee ee eine eee ee ee 


Author(s): Scott Book 

Project: _Yamabico Robot Control System 

Date: December 8, 1993 

Revised: March 4, 1994 

File Name: user.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains prototype for USER(). This is not required 
by the current implementation of the new MML system since user 
and kernel are two seperate object modules that are loaded at 
seperate locations. However, if the modules are combined in the 
future, main() can call USER() directly instead of user(). The 
prototype would then be required. 


aa a a aa a lia aaah lll tail | 


#ifndef . USER_H 


#define USER_H 


ial Cri ect ag ee eget ey we ee ee ere eee ee ea ee ne ee yee 


Function USER() is the user system program that receives control from main() 
after the robot’s sub-systems have been initialized. 


ne get ene tenn mee et ven eee ner ee er ee Cee ne ne a teen ee wef 


void USER(); 


#endif 


B. USER.C 
eee on eee cn ee en ee eer ere m eeoceS Sree eee eee 


Author(s): Scott Book 

Project: Yamabico Robot Control System 

Date: December 8, 1993 

Revised: March 4, 1994 

File Name: user.c 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file contains a sample user program that can be used with 
the new MML system created using ANSI C. The purpose of this 
program is to provide an interactive menu system to control the 
robot using immediate commands only. It was also used to test 
the implementation of the immediate commands. Since the user 
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module is loaded at 0x334000, USER() must be the first function 
defined. 


CE ee eee me rie mpg a pega ee ng ne ner ee ee ie 


#include “definitions.h” 
#include “iosys.h” 
#include “serial.h” 
#include “motion.h” 
#include “motiontrace.h” 
#include “compatability.h” 
#include “user.h” 


#define ESC Ox1b 


| halal alittle al Pala il! lalallala! shah! slags" alialiagigs I BIOL II INES 


The following section presents the prototypes for the encapsulated function(s) 
in this module. 


acacia tea ial halad ghee aaa ada IE SEI 


static void body(); 


| aaah aaa iach iia heat aad ca sla a il ial inl cB IBA AN BRN SAAN 


PUBLIC SECTION 


The following section defines the publicly accessible USER() routine that 
is called from main(). 


alata ali i alk alk lia li glo ll hl aaa ick Dhl ARN Sh hal) 


| het ala allie nolan ath sl igi gc pl i e o i A 


Function USER() is essentially a shell routine that calls body(). The reason 
for this approach is to prevent the compiler from changing the starting 
address of USER() by storing variables and/or strings before the routine. 


lalla ails i a al aaa cece taal elt) eh IA 


void USER(){ 
body(); 
} 


| haat cia aliclaimhahalahahalatertedntcechchechehoel gh shhh hchchrdohch SD SAR ALA AS ADEA SASSI ABLA DS BAA SRD IRS 


PRIVATE SECTION 


The following section defines the encapsulated function definition(s) used 
within this module. 
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Sania eae ae ee oe Cree ie ey ie ee oc re ee ee mee OOOO S Baer me ee ey 


Mae Uae ALO Faas SOS a ORR UT oe ee eee eae tae en eae eng ee eee 


Function body() is the work horse routine of this user program. It provides 

the menu system to interact with MML’s immediate commands. The routine 
purposely uses the older MML-10 commad calls in order to demonstrate use of 
the compatability macros defined in compatability.h 


cele og ane eee or nee ae ee coke ee pee eee cee See ee ee ee eee rere ee ey 


Static void body(){ 
int watt; 
char KeyBoardhHit; 
CONFIGURATION VehicleConfiguration; 


KeyBoardhHit = 0; 
motion_on(); 


do{ 
PutStr(‘AnHit: ESC to Terminate Program’); 
PutStr(n C to Change Robot’s Configuration ‘); 
PutStr(\n = R to Report Robot’s Current Configuration “); 
PutStr(‘An = -v_ to Change Linear Velocity’); 
PutStr(An sr to Change Rotational Velocity “); 
PutStr(A\n a to Change Linear Acceleration’); 
PutStr(\n = b to Change Rotational Acceleration “); 
PutStr(\n S to Change the Size Constant ‘); 
PutStr(An = s_to Skip the next Sequential Instruction “); 
PutStr(An — hh to Halt the Robot “); 
PutStr(A\n —=c to Resume the Robot ‘); 
KeyBoardHit = GetConsole(); 
PutConsole(KeyBoardHit); 


switch(KeyBoardHit){ 

case ESC: 
stop0(); 
PutStr(‘An\nThe Total Distance Traveled is: “); 
PutReal(path_length(),4); 
break; 

case ‘C’: 
set_rob0O(get_initial_position()); 
break; 

case ‘R’: 
get_rob0(&VehicleConfiguration); 
report_configuration(VehicleConfiguration); 
break; 

case ‘V’: 
PutStr(‘\a\n\nEnter Desired Linear Velocity: “); 
speed0(GetReal()); 
MotionTraceEnable(3); /“ Log the data every 3 Motion Control Cycles. */ 
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break; 
case 'r’: 
PutStr(‘\a\n\nEnter Desired Rotational Velocity: “); 
r_speed0(GetReal()); 
break; 
case ‘a’: 
PutStr(‘\a\n\nEnter Desired Linear Acceleration: “); 
acc0(GetReal()); 
break; 
case ‘b’: 
PutStr(“\a\n\nEnter Desired Rotational Acceleration: “); 
r_acc0(GetReal()); 
break; 
case ‘S’: 
PutStr(‘\a\n\nEnter Desired Size Constant: ‘); 
size_const(GetReal()); 
break; 
case ‘Ss’: 
skip(); 
break; 
case ‘h’: 
halt(); 
break; 
case ‘Cc’: 
resume(); 
break; 
default: 
break; 


} 
}while(KeyBoardHit l= ESC); 


DownLoadMotionData(); 


PutStr(‘An\nProgram Terminated.\a\n\n’); 


motion_off(); 


for(wait=0; wait<0x1000; wait++) 


COMPATABILITY.H 
| hated tithe heerlen alii! ale chead ol Lol alinkal ed Dh el oh DBE NE NEES 


Author(s): Scott Book 
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Project: _Yamabico Robot Control System 

Date: January 18, 1994 

Revised: March 4, 1994 

File Name: compatability.h 

Environment: GCC ANSI C compiler for the motorola 68020 processor 

Description: This file provides macros to convert old MML-10 immediate 
command calls into the new command calls. This allows user 
programs created for MML-10 to be used on the new system. 


CE rei rise Gg ere ee ee ere rr etn e oe ee cn age ey 


#ifndef_ COMPATABILITY_H 
#define — COMPATABILITY_H 


#include “definitions.h” 
#include “motion.h” 


#define set_rob0(P) SetRobotConfiguration(P) 
#define get_rob0(P) GetRobotConfiguration(P) 
#define stop0() Stop() 

#define speed0(P) SetLinearVelocity(P) 
#define r_speed0(P) SetRotationalVelocity(P) 
#define acc0O(P) SetLinearAcceleration(P) 
#define r_acc0(P) SetRotationalAcceleration(P) 
#define size_const(P) SetSizeConstant(P) 
#define path_length() GetTotalDistance() 
#define skip() SkipPathElement() 

#define halt() HalttMotion() 

#define resume() ResumeMotion() 

#define get_initial_position() SetlnitialPosition() 
#define report_configuration(P) ReportRobotConfiguration(P) 
#define motion_on() MotionOn() 


#define motion_off() MotionOff() 
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#endif 
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